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
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 "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  SigAltiGPS = 5.;
51  SigAltiAltimetre = 5.;
54  last_gps_alt = 0.;
55  last_baro_alt = 0.;
56 
57  kalmanInit(&alt_filter);
58 }
59 
60 void alt_filter_periodic(void) {
61  // estimation at each step
62  kalmanEstimation(&alt_filter,0.);
63 
64  // update on new data
65  float ga = (float)gps.hmsl / 1000.;
69  }
70  if (ga != last_gps_alt && GpsFixValid()) {
71  kalmanCorrectionGPS(&alt_filter, ga);
72  last_gps_alt = ga;
73  }
74 
75  RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, &baro_ets_altitude,
76  &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]),
77  &(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2])));
78 
79 }
80 
81 
82 /*************************************************************************
83  *
84  * Filter Initialisation
85  *
86  *************************************************************************/
87 
88 
90 
91  k->W[0][0] = MarcheAleaAccelerometre*MarcheAleaAccelerometre; k->W[0][1] = 0;
92  k->W[1][0] = 0; k->W[1][1] = MarcheAleaBiaisAltimetre*MarcheAleaBiaisAltimetre;
93 
94  k->X[0] = 0;
95  k->X[1] = 0;
96  k->X[2] = 0;
97 
98  k->P[0][0] = 1; k->P[0][1] = 0; k->P[0][2] = 0;
99  k->P[1][0] = 0; k->P[1][1] = 1; k->P[1][2] = 0;
100  k->P[2][0] = 0; k->P[2][1] = 0; k->P[2][2] = 0.0001;
101 
102  k->Te = (1./60.);
103 
104  // System dynamic
105  k->Ad[0][0] = 1; k->Ad[0][1] = k->Te; k->Ad[0][2] = 0;
106  k->Ad[1][0] = 0; k->Ad[1][1] = 1; k->Ad[1][2] = 0;
107  k->Ad[2][0] = 0; k->Ad[2][1] = 0; k->Ad[2][2] = 1;
108 
109  // System command
110  k->Bd[0] = pow(k->Te,2)/2;
111  k->Bd[1] = k->Te;
112  k->Bd[2] = 0;
113 
114  k->Md[0][0] = pow(k->Te, 1.5)/2; k->Md[0][1] = 0;
115  k->Md[1][0] = pow(k->Te, 0.5); k->Md[1][1] = 0;
116  k->Md[2][0] = 0; k->Md[2][1] = pow(k->Te, 0.5);
117 }
118 
119 
120 /*************************************************************************
121  *
122  * Estimation
123  *
124  *************************************************************************/
125 
126 
127 void kalmanEstimation(TypeKalman *k, float accVert){
128 
129 
130  int i,j;
131  float I[3][3]; // matrices temporaires
132  float J[3][2];
133 
134  // Calcul de X
135  // X(k+1) = Ad*X(k) + Bd*U(k)
136  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;
137  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;
138  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;
139 
140  // Calcul de P
141  // P(k+1) = Ad*P(k)*Ad' + Md*W*Md'
142  for(i=0;i<3;i++){
143  for (j=0;j<3;j++){
144  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];
145  }
146  }
147 
148  for(i=0;i<3;i++){
149  for (j=0;j<3;j++){
150  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];
151  }
152  }
153 
154  for(i=0;i<3;i++){
155  for (j=0;j<2;j++){
156  J[i][j] = k->Md[i][0]*k->W[0][j] + k->Md[i][1]*k->W[1][j];
157  }
158  }
159 
160  for(i=0;i<3;i++){
161  for (j=0;j<3;j++){
162  k->P[i][j] = k->P[i][j] + J[i][0]*k->Md[j][0] + J[i][1]*k->Md[j][1];
163  }
164  }
165 
166 }
167 
168 /*************************************************************************
169  *
170  * Correction GPS
171  *
172  *************************************************************************/
173 
174 void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est l'altitude telle qu'elle est mesurée par le GPS
175 
176  int i, j, div;
177  float I[3][3]; // matrice temporaire
178 
179  float Kf[3] = { 0., 0., 0. };
180 
181  // calcul de Kf
182  // C = [1 0 0]
183  // div = C*P*C' + R
184  div = k->P[0][0] + SigAltiGPS*SigAltiGPS;
185 
186  if (fabs(div) > 1e-5) {
187  // Kf = P*C'*inv(div)
188  Kf[0] = k->P[0][0] / div;
189  Kf[1] = k->P[1][0] / div;
190  Kf[2] = k->P[2][0] / div;
191 
192  // calcul de X
193  // X = X + Kf*(meas - C*X)
194  float constante = k->X[0];
195  for(i=0;i<3;i++){
196  k->X[i] = k->X[i]+Kf[i]*(altitude_gps - constante);
197  }
198 
199  // calcul de P
200  // P = P - Kf*C*P
201  I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = 0;
202  I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = 0;
203  I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = 0;
204 
205  for(i=0;i<3;i++){
206  for (j=0;j<3;j++){
207  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];
208  }
209  }
210  }
211 
212 }
213 
214 /*************************************************************************
215  *
216  * Correction altimètre
217  *
218  *************************************************************************/
219 
220 void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){
221 
222  int i, j, div;
223  float I[3][3]; // matrice temporaire
224 
225  float Kf[3] = { 0., 0., 0. };
226 
227  // calcul de Kf
228  // C = [1 0 1]
229  // div = C*P*C' + R
230  div = k->P[0][0] + k->P[2][0] + k->P[0][2] + k->P[2][2] + SigAltiAltimetre*SigAltiAltimetre;
231 
232  if (fabs(div) > 1e-5) {
233  // Kf = P*C'*inv(div)
234  Kf[0] = (k->P[0][0] + k->P[0][2]) / div;
235  Kf[1] = (k->P[1][0] + k->P[1][2]) / div;
236  Kf[2] = (k->P[2][0] + k->P[2][2]) / div;
237 
238  // calcul de X
239  // X = X + Kf*(meas - C*X)
240  float constante = k->X[0] + k->X[2];
241  for(i=0;i<3;i++){
242  k->X[i] = k->X[i]+Kf[i]*(altitude_altimetre - constante);
243  }
244 
245  // calcul de P
246  // P = P - Kf*C*P
247  I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = Kf[0];
248  I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = Kf[1];
249  I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = Kf[2];
250 
251  for(i=0;i<3;i++){
252  for (j=0;j<3;j++){
253  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];
254  }
255  }
256  }
257 
258 }
void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps)
Definition: alt_filter.c:174
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:67
float MarcheAleaBiaisAltimetre
Definition: alt_filter.c:36
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
void alt_filter_periodic(void)
Definition: alt_filter.c:60
TypeKalman alt_filter
Definition: alt_filter.c:31
void kalmanEstimation(TypeKalman *k, float accVert)
Definition: alt_filter.c:127
Driver for the EagleTree Systems Altitude Sensor.
Device independent GPS code (interface)
float Te
Definition: alt_filter.h:40
float baro_ets_altitude
Definition: baro_ets.c:106
float X[KALT_N_ETAT]
Definition: alt_filter.h:43
float last_gps_alt
Definition: alt_filter.c:46
float last_baro_alt
Definition: alt_filter.c:47
float W[KALT_N_ETAT-1][KALT_N_ETAT-1]
Definition: alt_filter.h:42
void alt_filter_init(void)
Definition: alt_filter.c:49
float SigAltiAltimetre
Definition: alt_filter.c:35
void kalmanInit(TypeKalman *k)
Definition: alt_filter.c:89
float Md[KALT_N_ETAT][KALT_N_ETAT-1]
Definition: alt_filter.h:46
float MarcheAleaAccelerometre
Definition: alt_filter.c:37
float SigAltiGPS
Definition: alt_filter.c:34
struct GpsState gps
global GPS state
Definition: gps.c:41
float Bd[KALT_N_ETAT]
Definition: alt_filter.h:44
void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre)
Definition: alt_filter.c:220
#define GpsFixValid()
Definition: gps.h:45