Paparazzi UAS  v5.18.0_stable 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
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
pprz_polyfit_float
void pprz_polyfit_float(float *x, float *y, int n, int p, float *c)
Polynomial regression.
Definition: pprz_polyfit_float.c:63
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
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
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_zero
static void float_vect_zero(float *a, const int n)
a = 0
Definition: pprz_algebra_float.h:539
float_mat_zero
static void float_mat_zero(float **a, int m, int n)
a = 0
Definition: pprz_algebra_float.h:647
mesonh.mesonh_atmosphere.T
int T
Definition: mesonh_atmosphere.py:46
pprz_polyfit_float.h
Polynomial regression.
p
static float p[2][2]
Definition: ins_alt_float.c:268