Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
50void 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
89void 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);
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
128static 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
144static int imin(int num1, int num2) {
145 return (num1 > num2 ) ? num2 : num1;
146}
167int 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
494void 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
529void 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:
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];
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];
570 float w[n_samples], _v[D_1][D_1];
572
573 // solve the system:
574
575 pprz_svd_float(AA, w, v, count, D_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
617void 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:
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];
652 float _AATAA[D_1][D_1];
654 float _PRIOR[D_1][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];
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");
694
695 // TODO: here, AATAA is used as float** - should be ok right?
696 // add the prior to AATAA:
698
699 DEBUG_PRINT("ATA+prior*I:\n");
701
702 float _INV_AATAA[D_1][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:
724
725 DEBUG_PRINT("GENERIC INV:\n");
727
728
729 //float_mat_inv_2d(INV_AATAA, _INV_AATAA);
730 float _PINV[D_1][count];
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];
740 MAT_MUL(D_1, count, 1, parameters, PINV, targets_all);
741
742 DEBUG_PRINT("parameters:\n");
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[22]
Definition ins_flow.c:249
static uint32_t s
static struct FloatVect3 H
uint16_t foo
Definition main_demo5.c:58
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
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
float b
Definition wedgebug.c:202