Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
pprz_matrix_decomp_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
30 #include <math.h>
31 #include <string.h>
32 
33 #if DEBUG_RANSAC
34 #include "stdio.h"
35 #define DEBUG_PRINT printf
36 #define DEBUG_MAT_PRINT MAT_PRINT
37 #else
38 #define DEBUG_PRINT(...)
39 #define DEBUG_MAT_PRINT(...)
40 #endif
41 
50 void pprz_cholesky_float(float **out, float **in, int n)
51 {
52  int i, j, k;
53  float _o[n][n];
54  MAKE_MATRIX_PTR(o, _o, n);
55 
56  float_mat_zero(o, n, n);
57  for (i = 0; i < n; i++) {
58  for (j = 0; j < (i + 1); j++) {
59  float s = 0;
60  for (k = 0; k < j; k++) {
61  s += o[i][k] * o[j][k];
62  }
63  if (i == j) {
64  o[i][j] = sqrtf(in[i][i] - s);
65  } else {
66  if (o[j][j] != 0) {
67  o[i][j] = 1.0 / o[j][j] * (in[i][j] - s);
68  } else {
69  o[i][j] = 0.0;
70  }
71  }
72  }
73  }
74  float_mat_copy(out, o, n, n);
75 }
76 
89 void pprz_qr_float(float **Q, float **R, float **in, int m, int n)
90 {
91  int i, k;
92  float _q[m][m][m];
93  float *q[m][m];
94  float _z[m][n], _z1[m][n], _z2[m][m];
95  MAKE_MATRIX_PTR(z, _z, m);
96  MAKE_MATRIX_PTR(z1, _z1, m);
97  MAKE_MATRIX_PTR(z2, _z2, m);
98  for (i = 0; i < m; i++) for (k = 0; k < m; k++) { q[i][k] = &_q[i][k][0]; }
99  float_mat_copy(z, in, m, n);
100  for (k = 0; k < n && k < m - 1; k++) {
101  float e[m], x[m], a, b;
102  float_mat_minor(z1, z, m, n, k);
103  float_mat_col(x, z1, m, k);
104  a = float_vect_norm(x, m);
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];
109  }
110  b = float_vect_norm(e, m);
111  float_vect_sdiv(e, e, b, m);
112  float_mat_vmul(q[k], e, m);
113  float_mat_mul(z, q[k], z1, m, m, n);
114  }
115  float_mat_copy(Q, q[0], m, m);
116  for (i = 1; i < n && i < m - 1; i++) {
117  float_mat_mul(z2, q[i], Q, m, m, m);
118  float_mat_copy(Q, z2, m, m);
119  }
120  float_mat_mul(R, Q, in, m, m, n);
122 }
123 
127 //Computes sqrt(a*a + b*b) without destructive underflow or overflow
128 static inline float pythag(float a, float b)
129 {
130  float absa, absb;
131  absa = fabsf(a);
132  absb = fabsf(b);
133  if (absa > absb) {
134  if (absa == 0) { return 0.0; }
135  return (absa * sqrtf(1.0 + (absb / absa) * (absb / absa)));
136  } else if (absb == 0.0) {
137  return 0.0;
138  } else {
139  return (absb * sqrtf(1.0 + (absa / absb) * (absa / absb)));
140  }
141 }
142 
143 
144 static int imin(int num1, int num2) {
145  return (num1 > num2 ) ? num2 : num1;
146 }
167 int pprz_svd_float(float **a, float *w, float **v, int m, int n)
168 {
169  /* Householder reduction to bidiagonal form. */
170  int flag, i, its, j, jj, k, l = 0, NM = 0;
171  float C, F, H, S, X, Y, Z, tmp;
172  float G = 0.0;
173  float Scale = 0.0;
174  float ANorm = 0.0;
175  float rv1[n];
176 
177  for (i = 0; i < n; ++i) {
178  l = i + 1;
179  rv1[i] = Scale * G;
180  G = 0.0;
181  S = 0.0;
182  Scale = 0.0;
183  if (i < m) {
184  for (k = i; k < m; ++k) {
185  Scale = Scale + fabsf(a[k][i]);
186  }
187  if (Scale != 0.0) {
188  for (k = i; k < m; ++k) {
189  a[k][i] = a[k][i] / Scale;
190  S = S + a[k][i] * a[k][i];
191  }
192  F = a[i][i];
193  G = sqrtf(S);
194  if (F > 0.0) {
195  G = -G;
196  }
197  H = F * G - S;
198  a[i][i] = F - G;
199  if (i != (n - 1)) {
200  for (j = l; j < n; ++j) {
201  S = 0.0;
202  for (k = i; k < m; ++k) {
203  S = S + a[k][i] * a[k][j];
204  }
205  if (H != 0.0) {
206  F = S / H;
207  } else {
208  F = 0.0;
209  }
210  for (k = i; k < m; ++k) {
211  a[k][j] = a[k][j] + F * a[k][i];
212  }
213  }
214  }
215  for (k = i; k < m; ++k) {
216  a[k][i] = Scale * a[k][i];
217  }
218  }
219  }
220 
221  w[i] = Scale * G;
222  G = 0.0;
223  S = 0.0;
224  Scale = 0.0;
225  if ((i < m) && (i != (n - 1))) {
226  for (k = l; k < n; ++k) {
227  Scale = Scale + fabsf(a[i][k]);
228  }
229  if (Scale != 0.0) {
230  for (k = l; k < n; ++k) {
231  a[i][k] = a[i][k] / Scale;
232  S = S + a[i][k] * a[i][k];
233  }
234  F = a[i][l];
235  G = sqrtf(S);
236  if (F > 0.0) {
237  G = -G;
238  }
239  H = F * G - S;
240  if (H == 0.0) {
241  H = 0.00001f;
242  }
243  a[i][l] = F - G;
244  for (k = l; k < n; ++k) {
245  rv1[k] = a[i][k] / H;
246  }
247  if (i != (m - 1)) {
248  for (j = l; j < m; ++j) {
249  S = 0.0;
250  for (k = l; k < n; ++k) {
251  S = S + a[j][k] * a[i][k];
252  }
253  for (k = l; k < n; ++k) {
254  a[j][k] = a[j][k] + S * rv1[k];
255  }
256  }
257  }
258  for (k = l; k < n; ++k) {
259  a[i][k] = Scale * a[i][k];
260  }
261  }
262  }
263  tmp = fabsf(w[i]) + fabsf(rv1[i]);
264  if (tmp > ANorm) {
265  ANorm = tmp;
266  }
267  }
268 
269  /* Accumulation of right-hand transformations. */
270  for (i = n - 1; i >= 0; --i) {
271  if (i < (n - 1)) {
272  if (G != 0.0) {
273  for (j = l; j < n; ++j) {
274  if (a[i][l] != 0) {
275  v[j][i] = (a[i][j] / a[i][l]) / G;
276  } else {
277  v[j][i] = 0.0;
278  }
279  }
280  for (j = l; j < n; ++j) {
281  S = 0.0;
282  for (k = l; k < n; ++k) {
283  S = S + a[i][k] * v[k][j];
284  }
285  for (k = l; k < n; ++k) {
286  v[k][j] = v[k][j] + S * v[k][i];
287  }
288  }
289  }
290  for (j = l; j < n; ++j) {
291  v[i][j] = 0.0;
292  v[j][i] = 0.0;
293  }
294  }
295  v[i][i] = 1.0;
296  G = rv1[i];
297  l = i;
298  }
299 
300  /* Accumulation of left-hand transformations. */
301  for (i = imin(m,n) - 1; i >= 0; --i) {
302  l = i + 1;
303  G = w[i];
304  if (i < (n - 1)) {
305  for (j = l; j < n; ++j) {
306  a[i][j] = 0.0;
307  }
308  }
309  if (G != 0.0) {
310  G = 1.0 / G;
311  if (i != (n - 1)) {
312  for (j = l; j < n; ++j) {
313  S = 0.0;
314  for (k = l; k < m; ++k) {
315  S = S + a[k][i] * a[k][j];
316  }
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];
320  }
321  }
322  }
323  for (j = i; j < m; ++j) {
324  a[j][i] = a[j][i] * G;
325  }
326  } else {
327  for (j = i; j < m; ++j) {
328  a[j][i] = 0.0;
329  }
330  }
331  a[i][i] = a[i][i] + 1.0;
332  }
333 
334  /* Diagonalization of the bidiagonal form.
335  Loop over singular values. */
336  for (k = (n - 1); k >= 0; --k) {
337  /* Loop over allowed iterations. */
338  for (its = 1; its <= 30; ++its) {
339  /* Test for splitting.
340  Note that rv1[0] is always zero. */
341  flag = true;
342  for (l = k; l >= 0; --l) {
343  NM = l - 1;
344  if ((fabsf(rv1[l]) + ANorm) == ANorm) {
345  flag = false;
346  break;
347  } else if ((fabsf(w[NM]) + ANorm) == ANorm) {
348  break;
349  }
350  }
351 
352  /* Cancellation of rv1[l], if l > 0; */
353  if (flag) {
354  C = 0.0;
355  S = 1.0;
356  for (i = l; i <= k; ++i) {
357  F = S * rv1[i];
358  if ((fabsf(F) + ANorm) != ANorm) {
359  G = w[i];
360  //H = sqrtf( F * F + G * G );
361  H = pythag(F, G);
362  w[i] = H;
363  if (H != 0) {
364  H = 1.0 / H;
365  } else {
366  H = 0;
367  }
368  C = (G * H);
369  S = -(F * H);
370  for (j = 0; j < m; ++j) {
371  Y = a[j][NM];
372  Z = a[j][i];
373  a[j][NM] = (Y * C) + (Z * S);
374  a[j][i] = -(Y * S) + (Z * C);
375  }
376  }
377  }
378  }
379  Z = w[k];
380  /* Convergence. */
381  if (l == k) {
382  /* Singular value is made nonnegative. */
383  if (Z < 0.0) {
384  w[k] = -Z;
385  for (j = 0; j < n; ++j) {
386  v[j][k] = -v[j][k];
387  }
388  }
389  break;
390  }
391 
392  if (its >= 30) {
393  // No convergence in 30 iterations
394  return 0;
395  }
396 
397  X = w[l];
398  NM = k - 1;
399  Y = w[NM];
400  G = rv1[NM];
401  H = rv1[k];
402  if (H * Y != 0) {
403  F = ((Y - Z) * (Y + Z) + (G - H) * (G + H)) / (2.0 * H * Y);
404  } else {
405  F = 0;
406  }
407  //G = sqrtf( F * F + 1.0 );
408  G = pythag(F, 1.0);
409  tmp = G;
410  if (F < 0.0) {
411  tmp = -tmp;
412  }
413  if ((F + tmp) != 0) {
414  F = ((X - Z) * (X + Z) + H * ((Y / (F + tmp)) - H)) / X;
415  } else {
416  F = 0;
417  }
418 
419  /* Next QR transformation. */
420  C = 1.0;
421  S = 1.0;
422  for (j = l; j <= NM; ++j) {
423  i = j + 1;
424  G = rv1[i];
425  Y = w[i];
426  H = S * G;
427  G = C * G;
428  //Z = sqrtf( F * F + H * H );
429  Z = pythag(F, H);
430  rv1[j] = Z;
431  if (Z != 0) {
432  C = F / Z;
433  S = H / Z;
434  } else {
435  C = 0;
436  S = 0;
437  }
438  F = (X * C) + (G * S);
439  G = -(X * S) + (G * C);
440  H = Y * S;
441  Y = Y * C;
442  for (jj = 0; jj < n; ++jj) {
443  X = v[jj][j];
444  Z = v[jj][i];
445  v[jj][j] = (X * C) + (Z * S);
446  v[jj][i] = -(X * S) + (Z * C);
447  }
448  //Z = sqrtf( F * F + H * H );
449  Z = pythag(F, H);
450  w[j] = Z;
451 
452  /* Rotation can be arbitrary if Z = 0. */
453  if (Z != 0.0) {
454  Z = 1.0 / Z;
455  C = F * Z;
456  S = H * Z;
457  }
458  F = (C * G) + (S * Y);
459  X = -(S * G) + (C * Y);
460  for (jj = 0; jj < m; ++jj) {
461  Y = a[jj][j];
462  Z = a[jj][i];
463  a[jj][j] = (Y * C) + (Z * S);
464  a[jj][i] = -(Y * S) + (Z * C);
465  }
466  }
467  rv1[l] = 0.0;
468  rv1[k] = F;
469  w[k] = X;
470  }
471  }
472 
473  return 1;
474 }
475 
494 void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l)
495 {
496  int i, j, jj, k;
497  float s;
498  float tmp[n];
499  for (k = 0; k < l; k++) { //Iterate on all column of b
500  for (j = 0; j < n; j++) { //Calculate UTB
501  s = 0.0;
502  if (w[j] != 0.0) { //Nonzero result only if wj is nonzero
503  for (i = 0; i < m; i++) { s += u[i][j] * b[i][k]; }
504  s /= w[j]; //This is the divide by wj
505  }
506  tmp[j] = s;
507  }
508  for (j = 0; j < n; j++) { //Matrix multiply by V to get answer
509  s = 0.0;
510  for (jj = 0; jj < n; jj++) { s += v[j][jj] * tmp[jj]; }
511  x[j][k] = s;
512  }
513  }
514 }
515 
516 
529 void fit_linear_model(float *targets, int D, float (*samples)[D], uint16_t count, bool use_bias, float *params,
530  float *fit_error)
531 {
532 
533  // We will solve systems of the form A x = b,
534  // where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias)
535  // and b = [nx1] vector with the target values.
536  // x in the system are the parameters for the linear regression function.
537 
538  // local vars for iterating, random numbers:
539  int sam, d;
540  uint16_t n_samples = count;
541  uint8_t D_1 = D + 1;
542  // ensure that n_samples is high enough to ensure a result for a single fit:
543  n_samples = (n_samples < D_1) ? D_1 : n_samples;
544  // n_samples should not be higher than count:
545  n_samples = (n_samples < count) ? n_samples : count;
546 
547  // initialize matrices and vectors for the full point set problem:
548  // this is used for determining inliers
549  float _AA[count][D_1];
550  MAKE_MATRIX_PTR(AA, _AA, count);
551  float _targets_all[count][1];
552  MAKE_MATRIX_PTR(targets_all, _targets_all, count);
553 
554  for (sam = 0; sam < count; sam++) {
555  for (d = 0; d < D; d++) {
556  AA[sam][d] = samples[sam][d];
557  }
558  if (use_bias) {
559  AA[sam][D] = 1.0f;
560  } else {
561  AA[sam][D] = 0.0f;
562  }
563  targets_all[sam][0] = targets[sam];
564  }
565 
566  // decompose A in u, w, v with singular value decomposition A = u * w * vT.
567  // u replaces A as output:
568  float _parameters[D_1][1];
569  MAKE_MATRIX_PTR(parameters, _parameters, D_1);
570  float w[n_samples], _v[D_1][D_1];
571  MAKE_MATRIX_PTR(v, _v, D_1);
572 
573  // solve the system:
574 
575  pprz_svd_float(AA, w, v, count, D_1);
576  pprz_svd_solve_float(parameters, AA, w, v, targets_all, count, D_1, 1);
577 
578  // used to determine the error of a set of parameters on the whole set:
579  float _bb[count][1];
580  MAKE_MATRIX_PTR(bb, _bb, count);
581  float _C[count][1];
582  MAKE_MATRIX_PTR(C, _C, count);
583 
584  // error is determined on the entire set
585  // bb = AA * parameters:
586  // TODO: error: AA has been replaced in the pprz_svd_float procedure by U!!!
587  MAT_MUL(count, D_1, 1, bb, AA, parameters);
588  // subtract bu_all: C = 0 in case of perfect fit:
589  MAT_SUB(count, 1, C, bb, targets_all);
590  *fit_error = 0;
591  for (sam = 0; sam < count; sam++) {
592  *fit_error += fabsf(C[sam][0]);
593  }
594  if (count > 0) {
595  *fit_error /= count;
596  }
597 
598  for (d = 0; d < D_1; d++) {
599  params[d] = parameters[d][0];
600  }
601 }
602 
603 
617 void fit_linear_model_prior(float *targets, int D, float (*samples)[D], uint16_t count, bool use_bias, float *priors,
618  float *params, float *fit_error)
619 {
620 
621  // We will solve systems of the form A x = b,
622  // where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias)
623  // and b = [nx1] vector with the target values.
624  // x in the system are the parameters for the linear regression function.
625 
626  // local vars for iterating, random numbers:
627  int sam, d;
628  uint16_t n_samples = count;
629  uint8_t D_1 = D + 1;
630 
631  if (D_1 != 2) {
632  DEBUG_PRINT("not yet implemented!!\n");
633  return;
634  }
635 
636  DEBUG_PRINT("n_samples = %d\n", n_samples);
637 
638  // ensure that n_samples is high enough to ensure a result for a single fit:
639  n_samples = (n_samples < D_1) ? D_1 : n_samples;
640  // n_samples should not be higher than count:
641  n_samples = (n_samples < count) ? n_samples : count;
642  count = n_samples;
643 
644  DEBUG_PRINT("n_samples = %d\n", n_samples);
645 
646  // initialize matrices and vectors for the full point set problem:
647  // this is used for determining inliers
648  float _AA[count][D_1];
649  MAKE_MATRIX_PTR(AA, _AA, count);
650  float _AAT[D_1][count];
651  MAKE_MATRIX_PTR(AAT, _AAT, D_1);
652  float _AATAA[D_1][D_1];
653  MAKE_MATRIX_PTR(AATAA, _AATAA, D_1);
654  float _PRIOR[D_1][D_1];
655  MAKE_MATRIX_PTR(PRIOR, _PRIOR, D_1);
656  for(d = 0; d < D; d++) {
657  PRIOR[d][d] = priors[d];
658  }
659  if(use_bias) {
660  PRIOR[D][D] = priors[D];
661  }
662  else {
663  PRIOR[D][D] = 1.0f;
664  }
665 
666  float _targets_all[count][1];
667  MAKE_MATRIX_PTR(targets_all, _targets_all, count);
668 
669  for (sam = 0; sam < count; sam++) {
670  for (d = 0; d < D; d++) {
671  AA[sam][d] = samples[sam][d];
672  }
673  if (use_bias) {
674  AA[sam][D] = 1.0f;
675  } else {
676  AA[sam][D] = 0.0f;
677  }
678  targets_all[sam][0] = targets[sam];
679  }
680 
681  DEBUG_PRINT("A:\n");
682  DEBUG_MAT_PRINT(count, D_1, AA);
683 
684  // make the pseudo-inverse matrix:
685  float_mat_transpose(AAT, AA, count, D_1);
686 
687  DEBUG_PRINT("AT:\n");
688  DEBUG_MAT_PRINT(D_1, count, AAT);
689 
690  float_mat_mul(AATAA, AAT, AA, D_1, count, D_1);
691 
692  DEBUG_PRINT("ATA:\n");
693  DEBUG_MAT_PRINT(D_1, D_1, AATAA);
694 
695  // TODO: here, AATAA is used as float** - should be ok right?
696  // add the prior to AATAA:
697  float_mat_sum(AATAA, AATAA, PRIOR, D_1, D_1);
698 
699  DEBUG_PRINT("ATA+prior*I:\n");
700  DEBUG_MAT_PRINT(D_1, D_1, AATAA);
701 
702  float _INV_AATAA[D_1][D_1];
703  MAKE_MATRIX_PTR(INV_AATAA, _INV_AATAA, D_1);
704 
705  /*
706  // 2-dimensional:
707  float det = AATAA[0][0] * AATAA[1][1] - AATAA[0][1] * AATAA[1][0];
708  if (fabsf(det) < 1e-4) {
709  printf("Not invertible\n");
710  return; // does this return go well?
711  } //not invertible
712 
713  INV_AATAA[0][0] = AATAA[1][1] / det;
714  INV_AATAA[0][1] = -AATAA[0][1] / det;
715  INV_AATAA[1][0] = -AATAA[1][0] / det;
716  INV_AATAA[1][1] = AATAA[0][0] / det;
717 
718  DEBUG_PRINT("INV assuming D_1 = 2:\n");
719  DEBUG_MAT_PRINT(D_1, D_1, INV_AATAA);
720  */
721 
722  // the AATAA matrix is square, so:
723  float_mat_invert(INV_AATAA, AATAA, D_1);
724 
725  DEBUG_PRINT("GENERIC INV:\n");
726  DEBUG_MAT_PRINT(D_1, D_1, INV_AATAA);
727 
728 
729  //float_mat_inv_2d(INV_AATAA, _INV_AATAA);
730  float _PINV[D_1][count];
731  MAKE_MATRIX_PTR(PINV, _PINV, D_1);
732  // TODO: what is the difference with float_mat_mul used above? Only that this is a matrix expansion?
733  MAT_MUL(D_1, D_1, count, PINV, INV_AATAA, AAT);
734 
735  DEBUG_PRINT("PINV:\n");
736  DEBUG_MAT_PRINT(D_1, count, PINV);
737 
738  float _parameters[D_1][1];
739  MAKE_MATRIX_PTR(parameters, _parameters, D_1);
740  MAT_MUL(D_1, count, 1, parameters, PINV, targets_all);
741 
742  DEBUG_PRINT("parameters:\n");
743  DEBUG_MAT_PRINT(D_1, 1, parameters);
744 
745  // used to determine the error of a set of parameters on the whole set:
746  float _bb[count][1];
747  MAKE_MATRIX_PTR(bb, _bb, count);
748  float _C[count][1];
749  MAKE_MATRIX_PTR(C, _C, count);
750 
751 
752  DEBUG_PRINT("A:\n");
753  DEBUG_MAT_PRINT(count, D_1, AA);
754 
755  // error is determined on the entire set
756  // bb = AA * parameters:
757  MAT_MUL(count, D_1, 1, bb, AA, parameters);
758 
759  // subtract bu_all: C = 0 in case of perfect fit:
760  MAT_SUB(count, 1, C, bb, targets_all);
761 
762  *fit_error = 0;
763  for (sam = 0; sam < count; sam++) {
764  *fit_error += fabsf(C[sam][0]);
765  }
766  *fit_error /= count;
767 
768  DEBUG_PRINT("Fit error = %f\n", *fit_error);
769 
770  for (d = 0; d < D_1; d++) {
771  params[d] = parameters[d][0];
772  }
773  DEBUG_PRINT("End of the function\n");
774 }
int n_samples
Definition: detect_gate.c:85
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
float parameters[20]
Definition: ins_flow.c:240
static uint32_t s
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.
#define DEBUG_PRINT(...)
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)
static float D
Definition: trilateration.c:35
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float b
Definition: wedgebug.c:202