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