Paparazzi UAS  v5.18.0_stable
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 
164 int pprz_svd_float(float **a, float *w, float **v, int m, int n)
165 {
166  /* Householder reduction to bidiagonal form. */
167  int flag, i, its, j, jj, k, l, NM;
168  float C, F, H, S, X, Y, Z, tmp;
169  float G = 0.0;
170  float Scale = 0.0;
171  float ANorm = 0.0;
172  float rv1[n];
173 
174  for (i = 0; i < n; ++i) {
175  l = i + 1;
176  rv1[i] = Scale * G;
177  G = 0.0;
178  S = 0.0;
179  Scale = 0.0;
180  if (i < m) {
181  for (k = i; k < m; ++k) {
182  Scale = Scale + fabsf(a[k][i]);
183  }
184  if (Scale != 0.0) {
185  for (k = i; k < m; ++k) {
186  a[k][i] = a[k][i] / Scale;
187  S = S + a[k][i] * a[k][i];
188  }
189  F = a[i][i];
190  G = sqrtf(S);
191  if (F > 0.0) {
192  G = -G;
193  }
194  H = F * G - S;
195  a[i][i] = F - G;
196  if (i != (n - 1)) {
197  for (j = l; j < n; ++j) {
198  S = 0.0;
199  for (k = i; k < m; ++k) {
200  S = S + a[k][i] * a[k][j];
201  }
202  if (H != 0.0) {
203  F = S / H;
204  } else {
205  F = 0.0;
206  }
207  for (k = i; k < m; ++k) {
208  a[k][j] = a[k][j] + F * a[k][i];
209  }
210  }
211  }
212  for (k = i; k < m; ++k) {
213  a[k][i] = Scale * a[k][i];
214  }
215  }
216  }
217 
218  w[i] = Scale * G;
219  G = 0.0;
220  S = 0.0;
221  Scale = 0.0;
222  if ((i < m) && (i != (n - 1))) {
223  for (k = l; k < n; ++k) {
224  Scale = Scale + fabsf(a[i][k]);
225  }
226  if (Scale != 0.0) {
227  for (k = l; k < n; ++k) {
228  a[i][k] = a[i][k] / Scale;
229  S = S + a[i][k] * a[i][k];
230  }
231  F = a[i][l];
232  G = sqrtf(S);
233  if (F > 0.0) {
234  G = -G;
235  }
236  H = F * G - S;
237  if (H == 0.0) {
238  H = 0.00001f;
239  }
240  a[i][l] = F - G;
241  for (k = l; k < n; ++k) {
242  rv1[k] = a[i][k] / H;
243  }
244  if (i != (m - 1)) {
245  for (j = l; j < m; ++j) {
246  S = 0.0;
247  for (k = l; k < n; ++k) {
248  S = S + a[j][k] * a[i][k];
249  }
250  for (k = l; k < n; ++k) {
251  a[j][k] = a[j][k] + S * rv1[k];
252  }
253  }
254  }
255  for (k = l; k < n; ++k) {
256  a[i][k] = Scale * a[i][k];
257  }
258  }
259  }
260  tmp = fabsf(w[i]) + fabsf(rv1[i]);
261  if (tmp > ANorm) {
262  ANorm = tmp;
263  }
264  }
265 
266  /* Accumulation of right-hand transformations. */
267  for (i = n - 1; i >= 0; --i) {
268  if (i < (n - 1)) {
269  if (G != 0.0) {
270  for (j = l; j < n; ++j) {
271  if (a[i][l] != 0) {
272  v[j][i] = (a[i][j] / a[i][l]) / G;
273  } else {
274  v[j][i] = 0.0;
275  }
276  }
277  for (j = l; j < n; ++j) {
278  S = 0.0;
279  for (k = l; k < n; ++k) {
280  S = S + a[i][k] * v[k][j];
281  }
282  for (k = l; k < n; ++k) {
283  v[k][j] = v[k][j] + S * v[k][i];
284  }
285  }
286  }
287  for (j = l; j < n; ++j) {
288  v[i][j] = 0.0;
289  v[j][i] = 0.0;
290  }
291  }
292  v[i][i] = 1.0;
293  G = rv1[i];
294  l = i;
295  }
296 
297  /* Accumulation of left-hand transformations. */
298  for (i = n - 1; i >= 0; --i) {
299  l = i + 1;
300  G = w[i];
301  if (i < (n - 1)) {
302  for (j = l; j < n; ++j) {
303  a[i][j] = 0.0;
304  }
305  }
306  if (G != 0.0) {
307  G = 1.0 / G;
308  if (i != (n - 1)) {
309  for (j = l; j < n; ++j) {
310  S = 0.0;
311  for (k = l; k < m; ++k) {
312  S = S + a[k][i] * a[k][j];
313  }
314  F = (S / a[i][i]) * G;
315  for (k = i; k < m; ++k) {
316  a[k][j] = a[k][j] + F * a[k][i];
317  }
318  }
319  }
320  for (j = i; j < m; ++j) {
321  a[j][i] = a[j][i] * G;
322  }
323  } else {
324  for (j = i; j < m; ++j) {
325  a[j][i] = 0.0;
326  }
327  }
328  a[i][i] = a[i][i] + 1.0;
329  }
330 
331  /* Diagonalization of the bidiagonal form.
332  Loop over singular values. */
333  for (k = (n - 1); k >= 0; --k) {
334  /* Loop over allowed iterations. */
335  for (its = 1; its <= 30; ++its) {
336  /* Test for splitting.
337  Note that rv1[0] is always zero. */
338  flag = true;
339  for (l = k; l >= 0; --l) {
340  NM = l - 1;
341  if ((fabsf(rv1[l]) + ANorm) == ANorm) {
342  flag = false;
343  break;
344  } else if ((fabsf(w[NM]) + ANorm) == ANorm) {
345  break;
346  }
347  }
348 
349  /* Cancellation of rv1[l], if l > 0; */
350  if (flag) {
351  C = 0.0;
352  S = 1.0;
353  for (i = l; i <= k; ++i) {
354  F = S * rv1[i];
355  if ((fabsf(F) + ANorm) != ANorm) {
356  G = w[i];
357  //H = sqrtf( F * F + G * G );
358  H = pythag(F, G);
359  w[i] = H;
360  if (H != 0) {
361  H = 1.0 / H;
362  } else {
363  H = 0;
364  }
365  C = (G * H);
366  S = -(F * H);
367  for (j = 0; j < m; ++j) {
368  Y = a[j][NM];
369  Z = a[j][i];
370  a[j][NM] = (Y * C) + (Z * S);
371  a[j][i] = -(Y * S) + (Z * C);
372  }
373  }
374  }
375  }
376  Z = w[k];
377  /* Convergence. */
378  if (l == k) {
379  /* Singular value is made nonnegative. */
380  if (Z < 0.0) {
381  w[k] = -Z;
382  for (j = 0; j < n; ++j) {
383  v[j][k] = -v[j][k];
384  }
385  }
386  break;
387  }
388 
389  if (its >= 30) {
390  // No convergence in 30 iterations
391  return 0;
392  }
393 
394  X = w[l];
395  NM = k - 1;
396  Y = w[NM];
397  G = rv1[NM];
398  H = rv1[k];
399  if (H * Y != 0) {
400  F = ((Y - Z) * (Y + Z) + (G - H) * (G + H)) / (2.0 * H * Y);
401  } else {
402  F = 0;
403  }
404  //G = sqrtf( F * F + 1.0 );
405  G = pythag(F, 1.0);
406  tmp = G;
407  if (F < 0.0) {
408  tmp = -tmp;
409  }
410  if ((F + tmp) != 0) {
411  F = ((X - Z) * (X + Z) + H * ((Y / (F + tmp)) - H)) / X;
412  } else {
413  F = 0;
414  }
415 
416  /* Next QR transformation. */
417  C = 1.0;
418  S = 1.0;
419  for (j = l; j <= NM; ++j) {
420  i = j + 1;
421  G = rv1[i];
422  Y = w[i];
423  H = S * G;
424  G = C * G;
425  //Z = sqrtf( F * F + H * H );
426  Z = pythag(F, H);
427  rv1[j] = Z;
428  if (Z != 0) {
429  C = F / Z;
430  S = H / Z;
431  } else {
432  C = 0;
433  S = 0;
434  }
435  F = (X * C) + (G * S);
436  G = -(X * S) + (G * C);
437  H = Y * S;
438  Y = Y * C;
439  for (jj = 0; jj < n; ++jj) {
440  X = v[jj][j];
441  Z = v[jj][i];
442  v[jj][j] = (X * C) + (Z * S);
443  v[jj][i] = -(X * S) + (Z * C);
444  }
445  //Z = sqrtf( F * F + H * H );
446  Z = pythag(F, H);
447  w[j] = Z;
448 
449  /* Rotation can be arbitrary if Z = 0. */
450  if (Z != 0.0) {
451  Z = 1.0 / Z;
452  C = F * Z;
453  S = H * Z;
454  }
455  F = (C * G) + (S * Y);
456  X = -(S * G) + (C * Y);
457  for (jj = 0; jj < m; ++jj) {
458  Y = a[jj][j];
459  Z = a[jj][i];
460  a[jj][j] = (Y * C) + (Z * S);
461  a[jj][i] = -(Y * S) + (Z * C);
462  }
463  }
464  rv1[l] = 0.0;
465  rv1[k] = F;
466  w[k] = X;
467  }
468  }
469 
470  return 1;
471 }
472 
491 void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l)
492 {
493  int i, j, jj, k;
494  float s;
495  float tmp[n];
496  for (k = 0; k < l; k++) { //Iterate on all column of b
497  for (j = 0; j < n; j++) { //Calculate UTB
498  s = 0.0;
499  if (w[j] != 0.0) { //Nonzero result only if wj is nonzero
500  for (i = 0; i < m; i++) { s += u[i][j] * b[i][k]; }
501  s /= w[j]; //This is the divide by wj
502  }
503  tmp[j] = s;
504  }
505  for (j = 0; j < n; j++) { //Matrix multiply by V to get answer
506  s = 0.0;
507  for (jj = 0; jj < n; jj++) { s += v[j][jj] * tmp[jj]; }
508  x[j][k] = s;
509  }
510  }
511 }
512 
513 
526 void fit_linear_model(float *targets, int D, float (*samples)[D], uint16_t count, bool use_bias, float *params,
527  float *fit_error)
528 {
529 
530  // We will solve systems of the form A x = b,
531  // where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias)
532  // and b = [nx1] vector with the target values.
533  // x in the system are the parameters for the linear regression function.
534 
535  // local vars for iterating, random numbers:
536  int sam, d;
537  uint16_t n_samples = count;
538  uint8_t D_1 = D + 1;
539  // ensure that n_samples is high enough to ensure a result for a single fit:
540  n_samples = (n_samples < D_1) ? D_1 : n_samples;
541  // n_samples should not be higher than count:
542  n_samples = (n_samples < count) ? n_samples : count;
543 
544  // initialize matrices and vectors for the full point set problem:
545  // this is used for determining inliers
546  float _AA[count][D_1];
547  MAKE_MATRIX_PTR(AA, _AA, count);
548  float _targets_all[count][1];
549  MAKE_MATRIX_PTR(targets_all, _targets_all, count);
550 
551  for (sam = 0; sam < count; sam++) {
552  for (d = 0; d < D; d++) {
553  AA[sam][d] = samples[sam][d];
554  }
555  if (use_bias) {
556  AA[sam][D] = 1.0f;
557  } else {
558  AA[sam][D] = 0.0f;
559  }
560  targets_all[sam][0] = targets[sam];
561  }
562 
563  // decompose A in u, w, v with singular value decomposition A = u * w * vT.
564  // u replaces A as output:
565  float _parameters[D_1][1];
566  MAKE_MATRIX_PTR(parameters, _parameters, D_1);
567  float w[n_samples], _v[D_1][D_1];
568  MAKE_MATRIX_PTR(v, _v, D_1);
569 
570  // solve the system:
571 
572  pprz_svd_float(AA, w, v, count, D_1);
573  pprz_svd_solve_float(parameters, AA, w, v, targets_all, count, D_1, 1);
574 
575  // used to determine the error of a set of parameters on the whole set:
576  float _bb[count][1];
577  MAKE_MATRIX_PTR(bb, _bb, count);
578  float _C[count][1];
579  MAKE_MATRIX_PTR(C, _C, count);
580 
581  // error is determined on the entire set
582  // bb = AA * parameters:
583  // TODO: error: AA has been replaced in the pprz_svd_float procedure by U!!!
584  MAT_MUL(count, D_1, 1, bb, AA, parameters);
585  // subtract bu_all: C = 0 in case of perfect fit:
586  MAT_SUB(count, 1, C, bb, targets_all);
587  *fit_error = 0;
588  for (sam = 0; sam < count; sam++) {
589  *fit_error += fabsf(C[sam][0]);
590  }
591  if (count > 0) {
592  *fit_error /= count;
593  }
594 
595  for (d = 0; d < D_1; d++) {
596  params[d] = parameters[d][0];
597  }
598 }
599 
600 
614 void fit_linear_model_prior(float *targets, int D, float (*samples)[D], uint16_t count, bool use_bias, float *priors,
615  float *params, float *fit_error)
616 {
617 
618  // We will solve systems of the form A x = b,
619  // where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias)
620  // and b = [nx1] vector with the target values.
621  // x in the system are the parameters for the linear regression function.
622 
623  // local vars for iterating, random numbers:
624  int sam, d;
625  uint16_t n_samples = count;
626  uint8_t D_1 = D + 1;
627 
628  if (D_1 != 2) {
629  DEBUG_PRINT("not yet implemented!!\n");
630  return;
631  }
632 
633  DEBUG_PRINT("n_samples = %d\n", n_samples);
634 
635  // ensure that n_samples is high enough to ensure a result for a single fit:
636  n_samples = (n_samples < D_1) ? D_1 : n_samples;
637  // n_samples should not be higher than count:
638  n_samples = (n_samples < count) ? n_samples : count;
639  count = n_samples;
640 
641  DEBUG_PRINT("n_samples = %d\n", n_samples);
642 
643  // initialize matrices and vectors for the full point set problem:
644  // this is used for determining inliers
645  float _AA[count][D_1];
646  MAKE_MATRIX_PTR(AA, _AA, count);
647  float _AAT[D_1][count];
648  MAKE_MATRIX_PTR(AAT, _AAT, D_1);
649  float _AATAA[D_1][D_1];
650  MAKE_MATRIX_PTR(AATAA, _AATAA, D_1);
651  float _PRIOR[D_1][D_1];
652  MAKE_MATRIX_PTR(PRIOR, _PRIOR, D_1);
653  for(d = 0; d < D; d++) {
654  PRIOR[d][d] = priors[d];
655  }
656  if(use_bias) {
657  PRIOR[D][D] = priors[D];
658  }
659  else {
660  PRIOR[D][D] = 1.0f;
661  }
662 
663  float _targets_all[count][1];
664  MAKE_MATRIX_PTR(targets_all, _targets_all, count);
665 
666  for (sam = 0; sam < count; sam++) {
667  for (d = 0; d < D; d++) {
668  AA[sam][d] = samples[sam][d];
669  }
670  if (use_bias) {
671  AA[sam][D] = 1.0f;
672  } else {
673  AA[sam][D] = 0.0f;
674  }
675  targets_all[sam][0] = targets[sam];
676  }
677 
678  DEBUG_PRINT("A:\n");
679  DEBUG_MAT_PRINT(count, D_1, AA);
680 
681  // make the pseudo-inverse matrix:
682  float_mat_transpose(AAT, AA, count, D_1);
683 
684  DEBUG_PRINT("AT:\n");
685  DEBUG_MAT_PRINT(D_1, count, AAT);
686 
687  float_mat_mul(AATAA, AAT, AA, D_1, count, D_1);
688 
689  DEBUG_PRINT("ATA:\n");
690  DEBUG_MAT_PRINT(D_1, D_1, AATAA);
691 
692  // TODO: here, AATAA is used as float** - should be ok right?
693  // add the prior to AATAA:
694  float_mat_sum(AATAA, AATAA, PRIOR, D_1, D_1);
695 
696  DEBUG_PRINT("ATA+prior*I:\n");
697  DEBUG_MAT_PRINT(D_1, D_1, AATAA);
698 
699  float _INV_AATAA[D_1][D_1];
700  MAKE_MATRIX_PTR(INV_AATAA, _INV_AATAA, D_1);
701 
702  /*
703  // 2-dimensional:
704  float det = AATAA[0][0] * AATAA[1][1] - AATAA[0][1] * AATAA[1][0];
705  if (fabsf(det) < 1e-4) {
706  printf("Not invertible\n");
707  return; // does this return go well?
708  } //not invertible
709 
710  INV_AATAA[0][0] = AATAA[1][1] / det;
711  INV_AATAA[0][1] = -AATAA[0][1] / det;
712  INV_AATAA[1][0] = -AATAA[1][0] / det;
713  INV_AATAA[1][1] = AATAA[0][0] / det;
714 
715  DEBUG_PRINT("INV assuming D_1 = 2:\n");
716  DEBUG_MAT_PRINT(D_1, D_1, INV_AATAA);
717  */
718 
719  // the AATAA matrix is square, so:
720  float_mat_invert(INV_AATAA, AATAA, D_1);
721 
722  DEBUG_PRINT("GENERIC INV:\n");
723  DEBUG_MAT_PRINT(D_1, D_1, INV_AATAA);
724 
725 
726  //float_mat_inv_2d(INV_AATAA, _INV_AATAA);
727  float _PINV[D_1][count];
728  MAKE_MATRIX_PTR(PINV, _PINV, D_1);
729  // TODO: what is the difference with float_mat_mul used above? Only that this is a matrix expansion?
730  MAT_MUL(D_1, D_1, count, PINV, INV_AATAA, AAT);
731 
732  DEBUG_PRINT("PINV:\n");
733  DEBUG_MAT_PRINT(D_1, count, PINV);
734 
735  float _parameters[D_1][1];
736  MAKE_MATRIX_PTR(parameters, _parameters, D_1);
737  MAT_MUL(D_1, count, 1, parameters, PINV, targets_all);
738 
739  DEBUG_PRINT("parameters:\n");
740  DEBUG_MAT_PRINT(D_1, 1, parameters);
741 
742  // used to determine the error of a set of parameters on the whole set:
743  float _bb[count][1];
744  MAKE_MATRIX_PTR(bb, _bb, count);
745  float _C[count][1];
746  MAKE_MATRIX_PTR(C, _C, count);
747 
748 
749  DEBUG_PRINT("A:\n");
750  DEBUG_MAT_PRINT(count, D_1, AA);
751 
752  // error is determined on the entire set
753  // bb = AA * parameters:
754  MAT_MUL(count, D_1, 1, bb, AA, parameters);
755 
756  // subtract bu_all: C = 0 in case of perfect fit:
757  MAT_SUB(count, 1, C, bb, targets_all);
758 
759  *fit_error = 0;
760  for (sam = 0; sam < count; sam++) {
761  *fit_error += fabsf(C[sam][0]);
762  }
763  *fit_error /= count;
764 
765  DEBUG_PRINT("Fit error = %f\n", *fit_error);
766 
767  for (d = 0; d < D_1; d++) {
768  params[d] = parameters[d][0];
769  }
770  DEBUG_PRINT("End of the function\n");
771 }
pprz_cholesky_float
void pprz_cholesky_float(float **out, float **in, int n)
Cholesky decomposition.
Definition: pprz_matrix_decomp_float.c:50
uint16_t
unsigned short uint16_t
Definition: types.h:16
fit_linear_model
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.
Definition: pprz_matrix_decomp_float.c:526
float_vect_norm
static float float_vect_norm(const float *a, const int n)
||a||
Definition: pprz_algebra_float.h:604
H
static struct FloatVect3 H
Definition: mag_calib_ukf.c:127
b
float b
Definition: wedgebug.c:202
pprz_svd_float
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
SVD decomposition.
Definition: pprz_matrix_decomp_float.c:164
float_mat_invert
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.
Definition: pprz_algebra_float.c:845
MAKE_MATRIX_PTR
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
Definition: pprz_algebra_float.h:635
float_mat_transpose_square
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
Definition: pprz_algebra_float.h:685
float_mat_vmul
static void float_mat_vmul(float **o, float *v, int n)
o = I - v v^T
Definition: pprz_algebra_float.h:825
float_mat_col
static void float_mat_col(float *o, float **a, int m, int c)
o = c-th column of matrix a[m x n]
Definition: pprz_algebra_float.h:839
s
static uint32_t s
Definition: light_scheduler.c:33
float_mat_copy
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
Definition: pprz_algebra_float.h:656
DEBUG_PRINT
#define DEBUG_PRINT(...)
Definition: pprz_matrix_decomp_float.c:38
mesonh.mesonh_atmosphere.Z
int Z
Definition: mesonh_atmosphere.py:45
pprz_matrix_decomp_float.h
Matrix decompositions in floating point.
pprz_algebra_float.h
Paparazzi floating point algebra.
pprz_svd_solve_float
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.
Definition: pprz_matrix_decomp_float.c:491
float_vect_sdiv
static void float_vect_sdiv(float *o, const float *a, const float s, const int n)
o = a / s
Definition: pprz_algebra_float.h:595
float_mat_minor
static void float_mat_minor(float **o, float **a, int m, int n, int d)
matrix minor
Definition: pprz_algebra_float.h:812
float_mat_zero
static void float_mat_zero(float **a, int m, int n)
a = 0
Definition: pprz_algebra_float.h:647
MAT_SUB
#define MAT_SUB(_i, _j, C, A, B)
Definition: pprz_simple_matrix.h:73
uint8_t
unsigned char uint8_t
Definition: types.h:14
Int32RMat::m
int32_t m[3 *3]
Definition: pprz_algebra_int.h:160
pythag
static float pythag(float a, float b)
Some SVD decomposition utility macros and functions.
Definition: pprz_matrix_decomp_float.c:128
z1
@ z1
Definition: discrete_ekf_no_north.c:34
n_samples
int n_samples
Definition: detect_gate.c:85
D
static float D
Definition: trilateration.c:35
AA
#define AA
Definition: LPC21xx.h:179
MAT_MUL
#define MAT_MUL(_i, _k, _j, C, A, B)
Definition: pprz_simple_matrix.h:46
float_mat_transpose
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
Definition: pprz_algebra_float.h:699
pprz_simple_matrix.h
Simple matrix helper macros.
pprz_qr_float
void pprz_qr_float(float **Q, float **R, float **in, int m, int n)
QR decomposition.
Definition: pprz_matrix_decomp_float.c:89
float_mat_sum
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
Definition: pprz_algebra_float.h:667
fit_linear_model_prior
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.
Definition: pprz_matrix_decomp_float.c:614
z2
@ z2
Definition: discrete_ekf_no_north.c:34
float_mat_mul
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
Definition: pprz_algebra_float.h:715
DEBUG_MAT_PRINT
#define DEBUG_MAT_PRINT(...)
Definition: pprz_matrix_decomp_float.c:39
mesonh.mesonh_atmosphere.Y
int Y
Definition: mesonh_atmosphere.py:44
mesonh.mesonh_atmosphere.X
int X
Definition: mesonh_atmosphere.py:43