35#define DEBUG_PRINT printf
36#define DEBUG_MAT_PRINT MAT_PRINT
38#define DEBUG_PRINT(...)
39#define DEBUG_MAT_PRINT(...)
57 for (i = 0; i < n; i++) {
58 for (
j = 0;
j < (i + 1);
j++) {
60 for (k = 0; k <
j; k++) {
61 s += o[i][k] * o[
j][k];
67 o[i][
j] = 1.0 / o[
j][
j] * (
in[i][
j] -
s);
94 float _z[m][n],
_z1[m][n],
_z2[m][m];
98 for (i = 0; i < m; i++)
for (k = 0; k < m; k++) { q[i][k] = &
_q[i][k][0]; }
100 for (k = 0; k < n && k < m - 1; k++) {
101 float e[m], x[m], a,
b;
105 if (
in[k][k] > 0) { a = -a; }
106 for (i = 0; i < m; i++) {
107 e[i] = (i == k) ? 1 : 0;
108 e[i] = x[i] + a * e[i];
116 for (i = 1; i < n && i < m - 1; i++) {
134 if (
absa == 0) {
return 0.0; }
136 }
else if (
absb == 0.0) {
171 float C, F,
H,
S, X, Y, Z,
tmp;
177 for (i = 0; i < n; ++i) {
184 for (k = i; k < m; ++k) {
188 for (k = i; k < m; ++k) {
189 a[k][i] = a[k][i] /
Scale;
190 S =
S + a[k][i] * a[k][i];
200 for (
j = l;
j < n; ++
j) {
202 for (k = i; k < m; ++k) {
203 S =
S + a[k][i] * a[k][
j];
210 for (k = i; k < m; ++k) {
211 a[k][
j] = a[k][
j] + F * a[k][i];
215 for (k = i; k < m; ++k) {
216 a[k][i] =
Scale * a[k][i];
225 if ((i < m) && (i != (n - 1))) {
226 for (k = l; k < n; ++k) {
230 for (k = l; k < n; ++k) {
231 a[i][k] = a[i][k] /
Scale;
232 S =
S + a[i][k] * a[i][k];
244 for (k = l; k < n; ++k) {
245 rv1[k] = a[i][k] /
H;
248 for (
j = l;
j < m; ++
j) {
250 for (k = l; k < n; ++k) {
251 S =
S + a[
j][k] * a[i][k];
253 for (k = l; k < n; ++k) {
254 a[
j][k] = a[
j][k] +
S *
rv1[k];
258 for (k = l; k < n; ++k) {
259 a[i][k] =
Scale * a[i][k];
270 for (i = n - 1; i >= 0; --i) {
273 for (
j = l;
j < n; ++
j) {
275 v[
j][i] = (a[i][
j] / a[i][l]) / G;
280 for (
j = l;
j < n; ++
j) {
282 for (k = l; k < n; ++k) {
283 S =
S + a[i][k] * v[k][
j];
285 for (k = l; k < n; ++k) {
286 v[k][
j] = v[k][
j] +
S * v[k][i];
290 for (
j = l;
j < n; ++
j) {
301 for (i =
imin(m,n) - 1; i >= 0; --i) {
305 for (
j = l;
j < n; ++
j) {
312 for (
j = l;
j < n; ++
j) {
314 for (k = l; k < m; ++k) {
315 S =
S + a[k][i] * a[k][
j];
317 F = (
S / a[i][i]) * G;
318 for (k = i; k < m; ++k) {
319 a[k][
j] = a[k][
j] + F * a[k][i];
323 for (
j = i;
j < m; ++
j) {
324 a[
j][i] = a[
j][i] * G;
327 for (
j = i;
j < m; ++
j) {
331 a[i][i] = a[i][i] + 1.0;
336 for (k = (n - 1); k >= 0; --k) {
342 for (l = k; l >= 0; --l) {
356 for (i = l; i <= k; ++i) {
370 for (
j = 0;
j < m; ++
j) {
373 a[
j][
NM] = (Y * C) + (Z *
S);
374 a[
j][i] = -(Y *
S) + (Z * C);
385 for (
j = 0;
j < n; ++
j) {
403 F = ((Y - Z) * (Y + Z) + (G -
H) * (G +
H)) / (2.0 *
H * Y);
413 if ((F +
tmp) != 0) {
414 F = ((X - Z) * (X + Z) +
H * ((Y / (F +
tmp)) -
H)) / X;
422 for (
j = l;
j <=
NM; ++
j) {
438 F = (X * C) + (G *
S);
439 G = -(X *
S) + (G * C);
442 for (
jj = 0;
jj < n; ++
jj) {
445 v[
jj][
j] = (X * C) + (Z *
S);
446 v[
jj][i] = -(X *
S) + (Z * C);
458 F = (C * G) + (
S * Y);
459 X = -(
S * G) + (C * Y);
460 for (
jj = 0;
jj < m; ++
jj) {
463 a[
jj][
j] = (Y * C) + (Z *
S);
464 a[
jj][i] = -(Y *
S) + (Z * C);
499 for (k = 0; k < l; k++) {
500 for (
j = 0;
j < n;
j++) {
503 for (i = 0; i < m; i++) {
s += u[i][
j] *
b[i][k]; }
508 for (
j = 0;
j < n;
j++) {
555 for (d = 0; d <
D; d++) {
598 for (d = 0; d <
D_1; d++) {
618 float *params,
float *fit_error)
656 for(d = 0; d <
D; d++) {
670 for (d = 0; d <
D; d++) {
770 for (d = 0; d <
D_1; d++) {
static void float_mat_vmul(float **o, float *v, int n)
o = I - v v^T
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
static void float_mat_col(float *o, float **a, int m, int c)
o = c-th column of matrix a[m x n]
static void float_vect_sdiv(float *o, const float *a, const float s, const int n)
o = a / s
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
static void float_mat_zero(float **a, int m, int n)
a = 0
static void float_mat_minor(float **o, float **a, int m, int n, int d)
matrix minor
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
static float float_vect_norm(const float *a, const int n)
||a||
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
static struct FloatVect3 H
Paparazzi floating point algebra.
void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l)
SVD based linear solver.
void fit_linear_model(float *targets, int D, float(*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error)
Fit a linear model from samples to target values.
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
SVD decomposition.
void fit_linear_model_prior(float *targets, int D, float(*samples)[D], uint16_t count, bool use_bias, float *priors, float *params, float *fit_error)
Fit a linear model from samples to target values with a prior.
static float pythag(float a, float b)
Some SVD decomposition utility macros and functions.
static int imin(int num1, int num2)
void pprz_qr_float(float **Q, float **R, float **in, int m, int n)
QR decomposition.
#define DEBUG_MAT_PRINT(...)
void pprz_cholesky_float(float **out, float **in, int n)
Cholesky decomposition.
Matrix decompositions in floating point.
Simple matrix helper macros.
#define MAT_MUL(_i, _k, _j, C, A, B)
#define MAT_SUB(_i, _j, C, A, B)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.