Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
29 #include <math.h>
30 #include <string.h>
31 
40 void pprz_cholesky_float(float **out, float **in, int n)
41 {
42  int i, j, k;
43  float _o[n][n];
44  MAKE_MATRIX_PTR(o, _o, n);
45 
46  float_mat_zero(o, n, n);
47  for (i = 0; i < n; i++) {
48  for (j = 0; j < (i + 1); j++) {
49  float s = 0;
50  for (k = 0; k < j; k++) {
51  s += o[i][k] * o[j][k];
52  }
53  o[i][j] = (i == j) ?
54  sqrtf(in[i][i] - s) :
55  (1.0 / o[j][j] * (in[i][j] - s));
56  }
57  }
58  float_mat_copy(out, o, n, n);
59 }
60 
73 void pprz_qr_float(float **Q, float **R, float **in, int m, int n)
74 {
75  int i, k;
76  float _q[m][m][m];
77  float *q[m][m];
78  float _z[m][n], _z1[m][n], _z2[m][m];
79  MAKE_MATRIX_PTR(z, _z, m);
80  MAKE_MATRIX_PTR(z1, _z1, m);
81  MAKE_MATRIX_PTR(z2, _z2, m);
82  for (i = 0; i < m; i++) for (k = 0; k < m; k++) { q[i][k] = &_q[i][k][0]; }
83  float_mat_copy(z, in, m, n);
84  for (k = 0; k < n && k < m - 1; k++) {
85  float e[m], x[m], a, b;
86  float_mat_minor(z1, z, m, n, k);
87  float_mat_col(x, z1, m, k);
88  a = float_vect_norm(x, m);
89  if (in[k][k] > 0) { a = -a; }
90  for (i = 0; i < m; i++) {
91  e[i] = (i == k) ? 1 : 0;
92  e[i] = x[i] + a * e[i];
93  }
94  b = float_vect_norm(e, m);
95  float_vect_sdiv(e, e, b, m);
96  float_mat_vmul(q[k], e, m);
97  float_mat_mul(z, q[k], z1, m, m, n);
98  }
99  float_mat_copy(Q, q[0], m, m);
100  for (i = 1; i < n && i < m - 1; i++) {
101  float_mat_mul(z2, q[i], Q, m, m, m);
102  float_mat_copy(Q, z2, m, m);
103  }
104  float_mat_mul(R, Q, in, m, m, n);
105  float_mat_transpose(Q, m);
106 }
107 
111 //Computes sqrt(a*a + b*b) without destructive underflow or overflow
112 static inline float pythag(float a, float b)
113 {
114  float absa, absb;
115  absa = fabsf(a);
116  absb = fabsf(b);
117  if (absa > absb) {
118  return (absa * sqrtf(1.0 + (absb / absa) * (absb / absa)));
119  } else if (absb == 0.0) {
120  return 0.0;
121  } else {
122  return (absb * sqrtf(1.0 + (absa / absb) * (absa / absb)));
123  }
124 }
125 
126 
147 int pprz_svd_float(float **a, float *w, float **v, int m, int n)
148 {
149  /* Householder reduction to bidiagonal form. */
150  int flag, i, its, j, jj, k, l, NM;
151  float C, F, H, S, X, Y, Z, tmp;
152  float G = 0.0;
153  float Scale = 0.0;
154  float ANorm = 0.0;
155  float rv1[n];
156 
157  for (i = 0; i < n; ++i) {
158  l = i + 1;
159  rv1[i] = Scale * G;
160  G = 0.0;
161  S = 0.0;
162  Scale = 0.0;
163  if (i < m) {
164  for (k = i; k < m; ++k) {
165  Scale = Scale + fabsf(a[k][i]);
166  }
167  if (Scale != 0.0) {
168  for (k = i; k < m; ++k) {
169  a[k][i] = a[k][i] / Scale;
170  S = S + a[k][i] * a[k][i];
171  }
172  F = a[i][i];
173  G = sqrtf(S);
174  if (F > 0.0) {
175  G = -G;
176  }
177  H = F * G - S;
178  a[i][i] = F - G;
179  if (i != (n - 1)) {
180  for (j = l; j < n; ++j) {
181  S = 0.0;
182  for (k = i; k < m; ++k) {
183  S = S + a[k][i] * a[k][j];
184  }
185  F = S / H;
186  for (k = i; k < m; ++k) {
187  a[k][j] = a[k][j] + F * a[k][i];
188  }
189  }
190  }
191  for (k = i; k < m; ++k) {
192  a[k][i] = Scale * a[k][i];
193  }
194  }
195  }
196 
197  w[i] = Scale * G;
198  G = 0.0;
199  S = 0.0;
200  Scale = 0.0;
201  if ((i < m) && (i != (n - 1))) {
202  for (k = l; k < n; ++k) {
203  Scale = Scale + fabsf(a[i][k]);
204  }
205  if (Scale != 0.0) {
206  for (k = l; k < n; ++k) {
207  a[i][k] = a[i][k] / Scale;
208  S = S + a[i][k] * a[i][k];
209  }
210  F = a[i][l];
211  G = sqrtf(S);
212  if (F > 0.0) {
213  G = -G;
214  }
215  H = F * G - S;
216  a[i][l] = F - G;
217  for (k = l; k < n; ++k) {
218  rv1[k] = a[i][k] / H;
219  }
220  if (i != (m - 1)) {
221  for (j = l; j < m; ++j) {
222  S = 0.0;
223  for (k = l; k < n; ++k) {
224  S = S + a[j][k] * a[i][k];
225  }
226  for (k = l; k < n; ++k) {
227  a[j][k] = a[j][k] + S * rv1[k];
228  }
229  }
230  }
231  for (k = l; k < n; ++k) {
232  a[i][k] = Scale * a[i][k];
233  }
234  }
235  }
236  tmp = fabsf(w[i]) + fabsf(rv1[i]);
237  if (tmp > ANorm) {
238  ANorm = tmp;
239  }
240  }
241 
242  /* Accumulation of right-hand transformations. */
243  for (i = n - 1; i >= 0; --i) {
244  if (i < (n - 1)) {
245  if (G != 0.0) {
246  for (j = l; j < n; ++j) {
247  v[j][i] = (a[i][j] / a[i][l]) / G;
248  }
249  for (j = l; j < n; ++j) {
250  S = 0.0;
251  for (k = l; k < n; ++k) {
252  S = S + a[i][k] * v[k][j];
253  }
254  for (k = l; k < n; ++k) {
255  v[k][j] = v[k][j] + S * v[k][i];
256  }
257  }
258  }
259  for (j = l; j < n; ++j) {
260  v[i][j] = 0.0;
261  v[j][i] = 0.0;
262  }
263  }
264  v[i][i] = 1.0;
265  G = rv1[i];
266  l = i;
267  }
268 
269  /* Accumulation of left-hand transformations. */
270  for (i = n - 1; i >= 0; --i) {
271  l = i + 1;
272  G = w[i];
273  if (i < (n - 1)) {
274  for (j = l; j < n; ++j) {
275  a[i][j] = 0.0;
276  }
277  }
278  if (G != 0.0) {
279  G = 1.0 / G;
280  if (i != (n - 1)) {
281  for (j = l; j < n; ++j) {
282  S = 0.0;
283  for (k = l; k < m; ++k) {
284  S = S + a[k][i] * a[k][j];
285  }
286  F = (S / a[i][i]) * G;
287  for (k = i; k < m; ++k) {
288  a[k][j] = a[k][j] + F * a[k][i];
289  }
290  }
291  }
292  for (j = i; j < m; ++j) {
293  a[j][i] = a[j][i] * G;
294  }
295  } else {
296  for (j = i; j < m; ++j) {
297  a[j][i] = 0.0;
298  }
299  }
300  a[i][i] = a[i][i] + 1.0;
301  }
302 
303  /* Diagonalization of the bidiagonal form.
304  Loop over singular values. */
305  for (k = (n - 1); k >= 0; --k) {
306  /* Loop over allowed iterations. */
307  for (its = 1; its <= 30; ++its) {
308  /* Test for splitting.
309  Note that rv1[0] is always zero. */
310  flag = true;
311  for (l = k; l >= 0; --l) {
312  NM = l - 1;
313  if ((fabsf(rv1[l]) + ANorm) == ANorm) {
314  flag = false;
315  break;
316  } else if ((fabsf(w[NM]) + ANorm) == ANorm) {
317  break;
318  }
319  }
320 
321  /* Cancellation of rv1[l], if l > 0; */
322  if (flag) {
323  C = 0.0;
324  S = 1.0;
325  for (i = l; i <= k; ++i) {
326  F = S * rv1[i];
327  if ((fabsf(F) + ANorm) != ANorm) {
328  G = w[i];
329  //H = sqrtf( F * F + G * G );
330  H = pythag(F, G);
331  w[i] = H;
332  H = 1.0 / H;
333  C = (G * H);
334  S = -(F * H);
335  for (j = 0; j < m; ++j) {
336  Y = a[j][NM];
337  Z = a[j][i];
338  a[j][NM] = (Y * C) + (Z * S);
339  a[j][i] = -(Y * S) + (Z * C);
340  }
341  }
342  }
343  }
344  Z = w[k];
345  /* Convergence. */
346  if (l == k) {
347  /* Singular value is made nonnegative. */
348  if (Z < 0.0) {
349  w[k] = -Z;
350  for (j = 0; j < n; ++j) {
351  v[j][k] = -v[j][k];
352  }
353  }
354  break;
355  }
356 
357  if (its >= 30) {
358  // No convergence in 30 iterations
359  return 0;
360  }
361 
362  X = w[l];
363  NM = k - 1;
364  Y = w[NM];
365  G = rv1[NM];
366  H = rv1[k];
367  F = ((Y - Z) * (Y + Z) + (G - H) * (G + H)) / (2.0 * H * Y);
368  //G = sqrtf( F * F + 1.0 );
369  G = pythag(F, 1.0);
370  tmp = G;
371  if (F < 0.0) {
372  tmp = -tmp;
373  }
374  F = ((X - Z) * (X + Z) + H * ((Y / (F + tmp)) - H)) / X;
375 
376  /* Next QR transformation. */
377  C = 1.0;
378  S = 1.0;
379  for (j = l; j <= NM; ++j) {
380  i = j + 1;
381  G = rv1[i];
382  Y = w[i];
383  H = S * G;
384  G = C * G;
385  //Z = sqrtf( F * F + H * H );
386  Z = pythag(F, H);
387  rv1[j] = Z;
388  C = F / Z;
389  S = H / Z;
390  F = (X * C) + (G * S);
391  G = -(X * S) + (G * C);
392  H = Y * S;
393  Y = Y * C;
394  for (jj = 0; jj < n; ++jj) {
395  X = v[jj][j];
396  Z = v[jj][i];
397  v[jj][j] = (X * C) + (Z * S);
398  v[jj][i] = -(X * S) + (Z * C);
399  }
400  //Z = sqrtf( F * F + H * H );
401  Z = pythag(F, H);
402  w[j] = Z;
403 
404  /* Rotation can be arbitrary if Z = 0. */
405  if (Z != 0.0) {
406  Z = 1.0 / Z;
407  C = F * Z;
408  S = H * Z;
409  }
410  F = (C * G) + (S * Y);
411  X = -(S * G) + (C * Y);
412  for (jj = 0; jj < m; ++jj) {
413  Y = a[jj][j];
414  Z = a[jj][i];
415  a[jj][j] = (Y * C) + (Z * S);
416  a[jj][i] = -(Y * S) + (Z * C);
417  }
418  }
419  rv1[l] = 0.0;
420  rv1[k] = F;
421  w[k] = X;
422  }
423  }
424 
425  return 1;
426 }
427 
446 void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l)
447 {
448  int i, j, jj, k;
449  float s;
450  float tmp[n];
451  for (k = 0; k < l; k++) { //Iterate on all column of b
452  for (j = 0; j < n; j++) { //Calculate UTB
453  s = 0.0;
454  if (w[j] != 0.0) { //Nonzero result only if wj is nonzero
455  for (i = 0; i < m; i++) { s += u[i][j] * b[i][k]; }
456  s /= w[j]; //This is the divide by wj
457  }
458  tmp[j] = s;
459  }
460  for (j = 0; j < n; j++) { //Matrix multiply by V to get answer
461  s = 0.0;
462  for (jj = 0; jj < n; jj++) { s += v[j][jj] * tmp[jj]; }
463  x[j][k] = s;
464  }
465  }
466 }
467 
void pprz_cholesky_float(float **out, float **in, int n)
Cholesky decomposition.
#define Q
Definition: hf_float.c:74
static void float_mat_transpose(float **a, int n)
transpose square matrix
static float float_vect_norm(const float *a, const int n)
||a||
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
static float pythag(float a, float b)
Some SVD decomposition utility macros and functions.
static void float_mat_copy(float **a, float **b, int m, int n)
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]
int32_t m[3 *3]
static void float_mat_vmul(float **o, float *v, int n)
o = I - v v^T
void pprz_qr_float(float **Q, float **R, float **in, int m, int n)
QR decomposition.
static void float_vect_sdiv(float *o, const float *a, const float s, const int n)
o = a / s
static void float_mat_minor(float **o, float **a, int m, int n, int d)
matrix minor
Paparazzi floating point algebra.
static void float_mat_zero(float **a, int m, int n)
a = 0
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.
Matrix decompositions in floating point.
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
SVD decomposition.
static struct FloatVect3 H
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b