Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
alt_filter.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 ENAC
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 
23 #include "modules/ins/alt_filter.h"
24 #include "subsystems/gps.h"
26 
27 #include "mcu_periph/uart.h"
28 #include "pprzlink/messages.h"
30 
32 
33 /* Kalman parameters */
34 float SigAltiGPS;
38 
39 /* Function declaration */
40 void kalmanInit(TypeKalman *k);
41 void kalmanEstimation(TypeKalman *k, float accVert);
42 void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps);
43 void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre);
44 
45 /* last measured values */
48 
49 void alt_filter_init(void)
50 {
51  SigAltiGPS = 5.;
52  SigAltiAltimetre = 5.;
55  last_gps_alt = 0.;
56  last_baro_alt = 0.;
57 
58  kalmanInit(&alt_filter);
59 }
60 
62 {
63  // estimation at each step
64  kalmanEstimation(&alt_filter, 0.);
65 
66  // update on new data
67  float ga = (float)gps.hmsl / 1000.;
71  }
72  if (ga != last_gps_alt && GpsFixValid()) {
73  kalmanCorrectionGPS(&alt_filter, ga);
74  last_gps_alt = ga;
75  }
76 
77  RunOnceEvery(6, DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, &baro_ets_altitude,
78  &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]),
79  &(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2])));
80 
81 }
82 
83 
84 /*************************************************************************
85  *
86  * Filter Initialisation
87  *
88  *************************************************************************/
89 
90 
92 {
93 
94  k->W[0][0] = MarcheAleaAccelerometre * MarcheAleaAccelerometre; k->W[0][1] = 0;
95  k->W[1][0] = 0; k->W[1][1] = MarcheAleaBiaisAltimetre * MarcheAleaBiaisAltimetre;
96 
97  k->X[0] = 0;
98  k->X[1] = 0;
99  k->X[2] = 0;
100 
101  k->P[0][0] = 1; k->P[0][1] = 0; k->P[0][2] = 0;
102  k->P[1][0] = 0; k->P[1][1] = 1; k->P[1][2] = 0;
103  k->P[2][0] = 0; k->P[2][1] = 0; k->P[2][2] = 0.0001;
104 
105  k->Te = (1. / 60.);
106 
107  // System dynamic
108  k->Ad[0][0] = 1; k->Ad[0][1] = k->Te; k->Ad[0][2] = 0;
109  k->Ad[1][0] = 0; k->Ad[1][1] = 1; k->Ad[1][2] = 0;
110  k->Ad[2][0] = 0; k->Ad[2][1] = 0; k->Ad[2][2] = 1;
111 
112  // System command
113  k->Bd[0] = pow(k->Te, 2) / 2;
114  k->Bd[1] = k->Te;
115  k->Bd[2] = 0;
116 
117  k->Md[0][0] = pow(k->Te, 1.5) / 2; k->Md[0][1] = 0;
118  k->Md[1][0] = pow(k->Te, 0.5); k->Md[1][1] = 0;
119  k->Md[2][0] = 0; k->Md[2][1] = pow(k->Te, 0.5);
120 }
121 
122 
123 /*************************************************************************
124  *
125  * Estimation
126  *
127  *************************************************************************/
128 
129 
130 void kalmanEstimation(TypeKalman *k, float accVert)
131 {
132 
133 
134  int i, j;
135  float I[3][3]; // matrices temporaires
136  float J[3][2];
137 
138  // Calcul de X
139  // X(k+1) = Ad*X(k) + Bd*U(k)
140  k->X[0] = k->Ad[0][0] * k->X[0] + k->Ad[0][1] * k->X[1] + k->Ad[0][2] * k->X[2] + k->Bd[0] * accVert;
141  k->X[1] = k->Ad[1][0] * k->X[0] + k->Ad[1][1] * k->X[1] + k->Ad[1][2] * k->X[2] + k->Bd[1] * accVert;
142  k->X[2] = k->Ad[2][0] * k->X[0] + k->Ad[2][1] * k->X[1] + k->Ad[2][2] * k->X[2] + k->Bd[2] * accVert;
143 
144  // Calcul de P
145  // P(k+1) = Ad*P(k)*Ad' + Md*W*Md'
146  for (i = 0; i < 3; i++) {
147  for (j = 0; j < 3; j++) {
148  I[i][j] = k->Ad[i][0] * k->P[0][j] + k->Ad[i][1] * k->P[1][j] + k->Ad[i][2] * k->P[2][j];
149  }
150  }
151 
152  for (i = 0; i < 3; i++) {
153  for (j = 0; j < 3; j++) {
154  k->P[i][j] = I[i][0] * k->Ad[j][0] + I[i][1] * k->Ad[j][1] + I[i][2] * k->Ad[j][2];
155  }
156  }
157 
158  for (i = 0; i < 3; i++) {
159  for (j = 0; j < 2; j++) {
160  J[i][j] = k->Md[i][0] * k->W[0][j] + k->Md[i][1] * k->W[1][j];
161  }
162  }
163 
164  for (i = 0; i < 3; i++) {
165  for (j = 0; j < 3; j++) {
166  k->P[i][j] = k->P[i][j] + J[i][0] * k->Md[j][0] + J[i][1] * k->Md[j][1];
167  }
168  }
169 
170 }
171 
172 /*************************************************************************
173  *
174  * Correction GPS
175  *
176  *************************************************************************/
177 
179  float altitude_gps) // altitude_gps est l'altitude telle qu'elle est mesurée par le GPS
180 {
181 
182  int i, j, div;
183  float I[3][3]; // matrice temporaire
184 
185  float Kf[3] = { 0., 0., 0. };
186 
187  // calcul de Kf
188  // C = [1 0 0]
189  // div = C*P*C' + R
190  div = k->P[0][0] + SigAltiGPS * SigAltiGPS;
191 
192  if (fabs(div) > 1e-5) {
193  // Kf = P*C'*inv(div)
194  Kf[0] = k->P[0][0] / div;
195  Kf[1] = k->P[1][0] / div;
196  Kf[2] = k->P[2][0] / div;
197 
198  // calcul de X
199  // X = X + Kf*(meas - C*X)
200  float constante = k->X[0];
201  for (i = 0; i < 3; i++) {
202  k->X[i] = k->X[i] + Kf[i] * (altitude_gps - constante);
203  }
204 
205  // calcul de P
206  // P = P - Kf*C*P
207  I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = 0;
208  I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = 0;
209  I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = 0;
210 
211  for (i = 0; i < 3; i++) {
212  for (j = 0; j < 3; j++) {
213  k->P[i][j] = k->P[i][j] - I[i][0] * k->P[0][j] - I[i][1] * k->P[1][j] - I[i][2] * k->P[2][j];
214  }
215  }
216  }
217 
218 }
219 
220 /*************************************************************************
221  *
222  * Correction altimètre
223  *
224  *************************************************************************/
225 
226 void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre)
227 {
228 
229  int i, j, div;
230  float I[3][3]; // matrice temporaire
231 
232  float Kf[3] = { 0., 0., 0. };
233 
234  // calcul de Kf
235  // C = [1 0 1]
236  // div = C*P*C' + R
237  div = k->P[0][0] + k->P[2][0] + k->P[0][2] + k->P[2][2] + SigAltiAltimetre * SigAltiAltimetre;
238 
239  if (fabs(div) > 1e-5) {
240  // Kf = P*C'*inv(div)
241  Kf[0] = (k->P[0][0] + k->P[0][2]) / div;
242  Kf[1] = (k->P[1][0] + k->P[1][2]) / div;
243  Kf[2] = (k->P[2][0] + k->P[2][2]) / div;
244 
245  // calcul de X
246  // X = X + Kf*(meas - C*X)
247  float constante = k->X[0] + k->X[2];
248  for (i = 0; i < 3; i++) {
249  k->X[i] = k->X[i] + Kf[i] * (altitude_altimetre - constante);
250  }
251 
252  // calcul de P
253  // P = P - Kf*C*P
254  I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = Kf[0];
255  I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = Kf[1];
256  I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = Kf[2];
257 
258  for (i = 0; i < 3; i++) {
259  for (j = 0; j < 3; j++) {
260  k->P[i][j] = k->P[i][j] - I[i][0] * k->P[0][j] - I[i][1] * k->P[1][j] - I[i][2] * k->P[2][j];
261  }
262  }
263  }
264 
265 }
void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps)
Definition: alt_filter.c:178
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float W[KALT_N_ETAT-1][KALT_N_ETAT-1]
Definition: alt_filter.h:41
static float I
Definition: trilateration.c:35
float MarcheAleaBiaisAltimetre
Definition: alt_filter.c:36
float Md[KALT_N_ETAT][KALT_N_ETAT-1]
Definition: alt_filter.h:45
float Bd[KALT_N_ETAT]
Definition: alt_filter.h:43
void alt_filter_periodic(void)
Definition: alt_filter.c:61
TypeKalman alt_filter
Definition: alt_filter.c:31
float Ad[KALT_N_ETAT][KALT_N_ETAT]
Definition: alt_filter.h:44
void kalmanEstimation(TypeKalman *k, float accVert)
Definition: alt_filter.c:130
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
static float J
Definition: trilateration.c:35
float Te
Definition: alt_filter.h:39
Driver for the EagleTree Systems Altitude Sensor.
Device independent GPS code (interface)
float baro_ets_altitude
Definition: baro_ets.c:106
float last_gps_alt
Definition: alt_filter.c:46
float last_baro_alt
Definition: alt_filter.c:47
void alt_filter_init(void)
Definition: alt_filter.c:49
float P[KALT_N_ETAT][KALT_N_ETAT]
Definition: alt_filter.h:40
float X[KALT_N_ETAT]
Definition: alt_filter.h:42
float SigAltiAltimetre
Definition: alt_filter.c:35
void kalmanInit(TypeKalman *k)
Definition: alt_filter.c:91
float MarcheAleaAccelerometre
Definition: alt_filter.c:37
float SigAltiGPS
Definition: alt_filter.c:34
struct GpsState gps
global GPS state
Definition: gps.c:75
void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre)
Definition: alt_filter.c:226
#define GpsFixValid()
Definition: gps.h:43