Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 #ifndef DOWNLINK_DEVICE
28 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
29 #endif
30 #include "mcu_periph/uart.h"
31 #include "messages.h"
33 
35 
36 /* Kalman parameters */
37 float SigAltiGPS;
41 
42 /* Function declaration */
43 void kalmanInit(TypeKalman *k);
44 void kalmanEstimation(TypeKalman *k, float accVert);
45 void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps);
46 void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre);
47 
48 /* last measured values */
51 
52 void alt_filter_init(void) {
53  SigAltiGPS = 5.;
54  SigAltiAltimetre = 5.;
57  last_gps_alt = 0.;
58  last_baro_alt = 0.;
59 
60  kalmanInit(&alt_filter);
61 }
62 
63 void alt_filter_periodic(void) {
64  // estimation at each step
65  kalmanEstimation(&alt_filter,0.);
66 
67  // update on new data
68  float ga = (float)gps.hmsl / 1000.;
72  }
73  if (ga != last_gps_alt && GpsFixValid()) {
74  kalmanCorrectionGPS(&alt_filter, ga);
75  last_gps_alt = ga;
76  }
77 
78  RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, &baro_ets_altitude,
79  &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]),
80  &(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2])));
81 
82 }
83 
84 
85 /*************************************************************************
86  *
87  * Filter Initialisation
88  *
89  *************************************************************************/
90 
91 
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  int i,j;
134  float I[3][3]; // matrices temporaires
135  float J[3][2];
136 
137  // Calcul de X
138  // X(k+1) = Ad*X(k) + Bd*U(k)
139  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;
140  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;
141  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;
142 
143  // Calcul de P
144  // P(k+1) = Ad*P(k)*Ad' + Md*W*Md'
145  for(i=0;i<3;i++){
146  for (j=0;j<3;j++){
147  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];
148  }
149  }
150 
151  for(i=0;i<3;i++){
152  for (j=0;j<3;j++){
153  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];
154  }
155  }
156 
157  for(i=0;i<3;i++){
158  for (j=0;j<2;j++){
159  J[i][j] = k->Md[i][0]*k->W[0][j] + k->Md[i][1]*k->W[1][j];
160  }
161  }
162 
163  for(i=0;i<3;i++){
164  for (j=0;j<3;j++){
165  k->P[i][j] = k->P[i][j] + J[i][0]*k->Md[j][0] + J[i][1]*k->Md[j][1];
166  }
167  }
168 
169 }
170 
171 /*************************************************************************
172  *
173  * Correction GPS
174  *
175  *************************************************************************/
176 
177 void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est l'altitude telle qu'elle est mesurée par le GPS
178 
179  int i, j, div;
180  float I[3][3]; // matrice temporaire
181 
182  float Kf[3] = { 0., 0., 0. };
183 
184  // calcul de Kf
185  // C = [1 0 0]
186  // div = C*P*C' + R
187  div = k->P[0][0] + SigAltiGPS*SigAltiGPS;
188 
189  if (fabs(div) > 1e-5) {
190  // Kf = P*C'*inv(div)
191  Kf[0] = k->P[0][0] / div;
192  Kf[1] = k->P[1][0] / div;
193  Kf[2] = k->P[2][0] / div;
194 
195  // calcul de X
196  // X = X + Kf*(meas - C*X)
197  float constante = k->X[0];
198  for(i=0;i<3;i++){
199  k->X[i] = k->X[i]+Kf[i]*(altitude_gps - constante);
200  }
201 
202  // calcul de P
203  // P = P - Kf*C*P
204  I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = 0;
205  I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = 0;
206  I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = 0;
207 
208  for(i=0;i<3;i++){
209  for (j=0;j<3;j++){
210  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];
211  }
212  }
213  }
214 
215 }
216 
217 /*************************************************************************
218  *
219  * Correction altimètre
220  *
221  *************************************************************************/
222 
223 void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){
224 
225  int i, j, div;
226  float I[3][3]; // matrice temporaire
227 
228  float Kf[3] = { 0., 0., 0. };
229 
230  // calcul de Kf
231  // C = [1 0 1]
232  // div = C*P*C' + R
233  div = k->P[0][0] + k->P[2][0] + k->P[0][2] + k->P[2][2] + SigAltiAltimetre*SigAltiAltimetre;
234 
235  if (fabs(div) > 1e-5) {
236  // Kf = P*C'*inv(div)
237  Kf[0] = (k->P[0][0] + k->P[0][2]) / div;
238  Kf[1] = (k->P[1][0] + k->P[1][2]) / div;
239  Kf[2] = (k->P[2][0] + k->P[2][2]) / div;
240 
241  // calcul de X
242  // X = X + Kf*(meas - C*X)
243  float constante = k->X[0] + k->X[2];
244  for(i=0;i<3;i++){
245  k->X[i] = k->X[i]+Kf[i]*(altitude_altimetre - constante);
246  }
247 
248  // calcul de P
249  // P = P - Kf*C*P
250  I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = Kf[0];
251  I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = Kf[1];
252  I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = Kf[2];
253 
254  for(i=0;i<3;i++){
255  for (j=0;j<3;j++){
256  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];
257  }
258  }
259  }
260 
261 }
void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps)
Definition: alt_filter.c:177
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float P[KALT_N_ETAT][KALT_N_ETAT]
Definition: alt_filter.h:41
float Ad[KALT_N_ETAT][KALT_N_ETAT]
Definition: alt_filter.h:45
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
float MarcheAleaBiaisAltimetre
Definition: alt_filter.c:39
void alt_filter_periodic(void)
Definition: alt_filter.c:63
TypeKalman alt_filter
Definition: alt_filter.c:34
void kalmanEstimation(TypeKalman *k, float accVert)
Definition: alt_filter.c:130
Device independent GPS code (interface)
float Te
Definition: alt_filter.h:40
float baro_ets_altitude
Definition: baro_ets.c:69
float X[KALT_N_ETAT]
Definition: alt_filter.h:43
float last_gps_alt
Definition: alt_filter.c:49
float last_baro_alt
Definition: alt_filter.c:50
float W[KALT_N_ETAT-1][KALT_N_ETAT-1]
Definition: alt_filter.h:42
void alt_filter_init(void)
Definition: alt_filter.c:52
float SigAltiAltimetre
Definition: alt_filter.c:38
void kalmanInit(TypeKalman *k)
Definition: alt_filter.c:92
float Md[KALT_N_ETAT][KALT_N_ETAT-1]
Definition: alt_filter.h:46
float MarcheAleaAccelerometre
Definition: alt_filter.c:40
float SigAltiGPS
Definition: alt_filter.c:37
struct GpsState gps
global GPS state
Definition: gps.c:31
float Bd[KALT_N_ETAT]
Definition: alt_filter.h:44
void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre)
Definition: alt_filter.c:223
#define GpsFixValid()
Definition: gps.h:44