Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
vf_extended_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2012 Gautier Hattenberger
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
33 
34 #define DEBUG_VFF_EXTENDED 1
35 
36 #if DEBUG_VFF_EXTENDED
37 #include "mcu_periph/uart.h"
38 #include "messages.h"
40 #endif
41 
42 /*
43 
44 X = [ z zdot accel_bias baro_offset ]
45 
46 temps :
47  propagate 86us
48  update 46us
49 
50 */
51 /* initial covariance diagonal */
52 #define INIT_PXX 1.
53 /* process noise */
54 #define ACCEL_NOISE 0.5
55 #define Qzz ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
56 #define Qzdotzdot ACCEL_NOISE * DT_VFILTER
57 #define Qbiasbias 1e-7
58 #define Qoffoff 1e-4
59 #define R_BARO 1.
60 #define R_ALT 0.1
61 #define R_OFFSET 1.
62 
63 float vff_z;
64 float vff_zdot;
65 float vff_bias;
66 float vff_offset;
68 
70 
71 float vff_z_meas;
73 
74 void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) {
75  vff_z = init_z;
76  vff_zdot = init_zdot;
77  vff_bias = init_accel_bias;
78  vff_offset = init_baro_offset;
79  int i, j;
80  for (i=0; i<VFF_STATE_SIZE; i++) {
81  for (j=0; j<VFF_STATE_SIZE; j++)
82  vff_P[i][j] = 0.;
83  vff_P[i][i] = INIT_PXX;
84  }
85 
86 }
87 
88 
89 /*
90 
91  F = [ 1 dt -dt^2/2 0
92  0 1 -dt 0
93  0 0 1 0
94  0 0 0 1 ];
95 
96  B = [ dt^2/2 dt 0 0]';
97 
98  Q = [ Qzz 0 0 0
99  0 Qzdotzdot 0 0
100  0 0 Qbiasbias 0
101  0 0 0 0 Qoffoff ];
102 
103  Xk1 = F * Xk0 + B * accel;
104 
105  Pk1 = F * Pk0 * F' + Q;
106 
107 */
108 void vff_propagate(float accel) {
109  /* update state */
110  vff_zdotdot = accel + 9.81 - vff_bias;
111  vff_z = vff_z + DT_VFILTER * vff_zdot;
112  vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
113  /* update covariance */
114  const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
115  const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
116  const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
117  const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
118  const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
119  const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
120  const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
121  const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
122  const float FPF22 = vff_P[2][2];
123  const float FPF33 = vff_P[3][3];
124 
125  vff_P[0][0] = FPF00 + Qzz;
126  vff_P[0][1] = FPF01;
127  vff_P[0][2] = FPF02;
128  vff_P[1][0] = FPF10;
129  vff_P[1][1] = FPF11 + Qzdotzdot;
130  vff_P[1][2] = FPF12;
131  vff_P[2][0] = FPF20;
132  vff_P[2][1] = FPF21;
133  vff_P[2][2] = FPF22 + Qbiasbias;
134  vff_P[3][3] = FPF33 + Qoffoff;
135 
136 #if DEBUG_VFF_EXTENDED
137  RunOnceEvery(10, DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z_meas_baro, &vff_z, &vff_zdot, &vff_zdotdot, &vff_bias, &vff_offset));
138 #endif
139 }
140 
141 /*
142  * Update sensor "with" offset (baro)
143  H = [1 0 0 -1];
144  // state residual
145  y = rangemeter - H * Xm;
146  // covariance residual
147  S = H*Pm*H' + R;
148  // kalman gain
149  K = Pm*H'*inv(S);
150  // update state
151  Xp = Xm + K*y;
152  // update covariance
153  Pp = Pm - K*H*Pm;
154 */
155 __attribute__ ((always_inline)) static inline void update_baro_conf(float z_meas, float conf) {
156  vff_z_meas_baro = z_meas;
157 
158  const float y = z_meas - vff_z + vff_offset;
159  const float S = vff_P[0][0] - vff_P[0][3] - vff_P[3][0] + vff_P[3][3] + conf;
160  const float K0 = (vff_P[0][0] - vff_P[0][3]) * 1/S;
161  const float K1 = (vff_P[1][0] - vff_P[1][3]) * 1/S;
162  const float K2 = (vff_P[2][0] - vff_P[2][3]) * 1/S;
163  const float K3 = (vff_P[3][0] - vff_P[3][3]) * 1/S;
164 
165  vff_z = vff_z + K0 * y;
166  vff_zdot = vff_zdot + K1 * y;
167  vff_bias = vff_bias + K2 * y;
168  vff_offset = vff_offset + K3 * y;
169 
170  const float P0 = vff_P[0][0] - vff_P[3][0];
171  const float P1 = vff_P[0][1] - vff_P[3][1];
172  const float P2 = vff_P[0][2] - vff_P[3][2];
173  const float P3 = vff_P[0][3] - vff_P[3][3];
174 
175  vff_P[0][0] -= K0 * P0;
176  vff_P[0][1] -= K0 * P1;
177  vff_P[0][2] -= K0 * P2;
178  vff_P[0][3] -= K0 * P3;
179  vff_P[1][0] -= K1 * P0;
180  vff_P[1][1] -= K1 * P1;
181  vff_P[1][2] -= K1 * P2;
182  vff_P[1][3] -= K1 * P3;
183  vff_P[2][0] -= K2 * P0;
184  vff_P[2][1] -= K2 * P1;
185  vff_P[2][2] -= K2 * P2;
186  vff_P[2][3] -= K2 * P3;
187  vff_P[3][0] -= K3 * P0;
188  vff_P[3][1] -= K3 * P1;
189  vff_P[3][2] -= K3 * P2;
190  vff_P[3][3] -= K3 * P3;
191 
192 }
193 
194 void vff_update_baro(float z_meas) {
195  update_baro_conf(z_meas, R_BARO);
196 }
197 
198 void vff_update_baro_conf(float z_meas, float conf) {
199  update_baro_conf(z_meas, conf);
200 }
201 
202 /*
203  * Update sensor "without" offset (gps, sonar)
204  H = [1 0 0 0];
205  // state residual
206  y = rangemeter - H * Xm;
207  // covariance residual
208  S = H*Pm*H' + R;
209  // kalman gain
210  K = Pm*H'*inv(S);
211  // update state
212  Xp = Xm + K*y;
213  // update covariance
214  Pp = Pm - K*H*Pm;
215 */
216 __attribute__ ((always_inline)) static inline void update_alt_conf(float z_meas, float conf) {
217  vff_z_meas = z_meas;
218 
219  const float y = z_meas - vff_z;
220  const float S = vff_P[0][0] + conf;
221  const float K0 = vff_P[0][0] * 1/S;
222  const float K1 = vff_P[1][0] * 1/S;
223  const float K2 = vff_P[2][0] * 1/S;
224  const float K3 = vff_P[3][0] * 1/S;
225 
226  vff_z = vff_z + K0 * y;
227  vff_zdot = vff_zdot + K1 * y;
228  vff_bias = vff_bias + K2 * y;
229  vff_offset = vff_offset + K3 * y;
230 
231  const float P0 = vff_P[0][0];
232  const float P1 = vff_P[0][1];
233  const float P2 = vff_P[0][2];
234  const float P3 = vff_P[0][3];
235 
236  vff_P[0][0] -= K0 * P0;
237  vff_P[0][1] -= K0 * P1;
238  vff_P[0][2] -= K0 * P2;
239  vff_P[0][3] -= K0 * P3;
240  vff_P[1][0] -= K1 * P0;
241  vff_P[1][1] -= K1 * P1;
242  vff_P[1][2] -= K1 * P2;
243  vff_P[1][3] -= K1 * P3;
244  vff_P[2][0] -= K2 * P0;
245  vff_P[2][1] -= K2 * P1;
246  vff_P[2][2] -= K2 * P2;
247  vff_P[2][3] -= K2 * P3;
248  vff_P[3][0] -= K3 * P0;
249  vff_P[3][1] -= K3 * P1;
250  vff_P[3][2] -= K3 * P2;
251  vff_P[3][3] -= K3 * P3;
252 }
253 
254 void vff_update_alt(float z_meas) {
255  update_alt_conf(z_meas, R_ALT);
256 }
257 
258 void vff_update_alt_conf(float z_meas, float conf) {
259  update_alt_conf(z_meas, conf);
260 }
261 
262 /*
263  * Update sensor offset (baro)
264  H = [0 0 0 1];
265  // state residual
266  y = rangemeter - H * Xm;
267  // covariance residual
268  S = H*Pm*H' + R;
269  // kalman gain
270  K = Pm*H'*inv(S);
271  // update state
272  Xp = Xm + K*y;
273  // update covariance
274  Pp = Pm - K*H*Pm;
275 */
276 __attribute__ ((always_inline)) static inline void update_offset_conf(float offset, float conf) {
277 
278  const float y = offset - vff_offset;
279  const float S = vff_P[3][3] + conf;
280  const float K0 = vff_P[0][3] * 1/S;
281  const float K1 = vff_P[1][3] * 1/S;
282  const float K2 = vff_P[2][3] * 1/S;
283  const float K3 = vff_P[3][3] * 1/S;
284 
285  vff_z = vff_z + K0 * y;
286  vff_zdot = vff_zdot + K1 * y;
287  vff_bias = vff_bias + K2 * y;
288  vff_offset = vff_offset + K3 * y;
289 
290  const float P0 = vff_P[3][0];
291  const float P1 = vff_P[3][1];
292  const float P2 = vff_P[3][2];
293  const float P3 = vff_P[3][3];
294 
295  vff_P[0][0] -= K0 * P0;
296  vff_P[0][1] -= K0 * P1;
297  vff_P[0][2] -= K0 * P2;
298  vff_P[0][3] -= K0 * P3;
299  vff_P[1][0] -= K1 * P0;
300  vff_P[1][1] -= K1 * P1;
301  vff_P[1][2] -= K1 * P2;
302  vff_P[1][3] -= K1 * P3;
303  vff_P[2][0] -= K2 * P0;
304  vff_P[2][1] -= K2 * P1;
305  vff_P[2][2] -= K2 * P2;
306  vff_P[2][3] -= K2 * P3;
307  vff_P[3][0] -= K3 * P0;
308  vff_P[3][1] -= K3 * P1;
309  vff_P[3][2] -= K3 * P2;
310  vff_P[3][3] -= K3 * P3;
311 }
312 
313 void vff_update_offset(float offset) {
314  update_offset_conf(offset, R_OFFSET);
315 }
316 
317 
318 void vff_realign(float z_meas) {
319  //vff_z = z_meas;
320  //vff_zdot = 0.;
321  //vff_offset = 0.;
322  vff_init(z_meas, 0., 0., 0.);
323 }
void vff_update_alt(float z_meas)
#define Qzdotzdot
static void update_baro_conf(float z_meas, float conf)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
Interface for extended vertical filter (in float).
#define R_BARO
float vff_z
float vff_zdotdot
void vff_update_alt_conf(float z_meas, float conf)
float vff_z_meas_baro
static void update_offset_conf(float offset, float conf)
float vff_bias
#define Qzz
#define VFF_STATE_SIZE
void vff_update_baro_conf(float z_meas, float conf)
static void update_alt_conf(float z_meas, float conf)
void vff_update_offset(float offset)
void vff_realign(float z_meas)
#define Qoffoff
#define K0
#define R_ALT
float vff_offset
#define R_OFFSET
void vff_update_baro(float z_meas)
float vff_zdot
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]
void vff_propagate(float accel)
float vff_z_meas
#define INIT_PXX
#define Qbiasbias