Paparazzi UAS  v5.14.0_stable-0-g3f680d1 Paparazzi is a free software Unmanned Aircraft System.
pprz_polyfit_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
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
19  */
20
30
63 void pprz_polyfit_float(float *x, float *y, int n, int p, float *c)
64 {
65  int i, j, k;
66
67  // Instead of solving directly (X'X)^-1 X' y
68  // let's build the matrices (X'X) and (X'y)
69  // Then element ij in (X'X) matrix is sum_{k=0,n-1} x_k^(i+j)
70  // and element i in (X'y) vector is sum_{k=0,n-1} x_k^i * y_k
71  // Finally we can solve the linear system (X'X).c = (X'y) using SVD decomposition
72
73  // First build a table of element S_i = sum_{k=0,n-1} x_k^i of dimension 2*p+1
74  float S[2 * p + 1];
75  float_vect_zero(S, 2 * p + 1);
76  // and a table of element T_i = sum_{k=0,n-1} x_k^i*y_k of dimension p+1
77  // make it a matrix for later use
78  float _T[p + 1][1];
79  MAKE_MATRIX_PTR(T, _T, p + 1);
80  float_mat_zero(T, p + 1, 1);
81  S[0] = n; // S_0 is always the number of input measurements
82  for (k = 0; k < n; k++) {
83  float x_tmp = x[k];
84  T[0][0] += y[k];
85  for (i = 1; i < 2 * p + 1; i++) {
86  S[i] += x_tmp; // add element to S_i
87  if (i < p + 1) {
88  T[i][0] += x_tmp * y[k]; // add element to T_i if i < p+1
89  }
90  x_tmp *= x[k]; // multiply x_tmp by current value of x
91  }
92  }
93  // Then build a [p+1 x p+1] matrix corresponding to (X'X) based on the S_i
94  // element ij of (X'X) is S_(i+j)
95  float _XtX[p + 1][p + 1];
96  MAKE_MATRIX_PTR(XtX, _XtX, p + 1);
97  for (i = 0; i < p + 1; i++) {
98  for (j = 0; j < p + 1; j++) {
99  XtX[i][j] = S[i + j];
100  }
101  }
102  // Solve linear system XtX.c = T after performing a SVD decomposition of XtX
103  // which is probably a bit overkill but looks really cool
104  float w[p + 1], _v[p + 1][p + 1];
105  MAKE_MATRIX_PTR(v, _v, p + 1);
106  pprz_svd_float(XtX, w, v, p + 1, p + 1);
107  float _c[p + 1][1];
108  MAKE_MATRIX_PTR(c_tmp, _c, p + 1);
109  pprz_svd_solve_float(c_tmp, XtX, w, v, T, p + 1, p + 1, 1);
110  // set output vector
111  for (i = 0; i < p + 1; i++) {
112  c[i] = c_tmp[i][0];
113  }
114
115 }
116
117
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
Paparazzi floating point algebra.
static void float_vect_zero(float *a, const int n)
a = 0
void pprz_polyfit_float(float *x, float *y, int n, int p, float *c)
Polynomial regression.
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.
Polynomial regression.
int pprz_svd_float(float **a, float *w, float **v, int m, int n)
SVD decomposition.
static float p[2][2]