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
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 #include "generated/airframe.h"
34 #include "std.h"
35 
36 #ifndef DEBUG_VFF_EXTENDED
37 #define DEBUG_VFF_EXTENDED 0
38 #else
40 #endif
41 
42 #if DEBUG_VFF_EXTENDED
43 #include "mcu_periph/uart.h"
44 #include "messages.h"
46 #endif
47 
48 #ifndef INS_PROPAGATE_FREQUENCY
49 #ifdef AHRS_PROPAGATE_FREQUENCY
50 #define INS_PROPAGATE_FREQUENCY AHRS_PROPAGATE_FREQUENCY
51 #else
52 #define INS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
53 #endif
54 #endif
56 
57 #define DT_VFILTER (1./(INS_PROPAGATE_FREQUENCY))
58 
59 
60 /*
61 
62 X = [ z zdot accel_bias baro_offset ]
63 
64 temps :
65  propagate 86us
66  update 46us
67 
68 */
69 /* initial covariance diagonal */
70 #define INIT_PXX 1.
71 /* process noise */
72 #define ACCEL_NOISE 0.5
73 #define Qzz ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
74 #define Qzdotzdot ACCEL_NOISE * DT_VFILTER
75 #define Qbiasbias 1e-7
76 #define Qoffoff 1e-4
77 #define R_BARO 1.
78 #define R_ALT 0.1
79 #define R_OFFSET 1.
80 
82 
83 #if PERIODIC_TELEMETRY
85 
86 static void send_vffe(void) {
87  DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice,
89  &vff.z, &vff.zdot, &vff.zdotdot,
90  &vff.bias, &vff.offset);
91 }
92 #endif
93 
94 void vff_init_zero(void) {
95  vff_init(0., 0., 0., 0.);
96 }
97 
98 void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) {
99  vff.z = init_z;
100  vff.zdot = init_zdot;
101  vff.bias = init_accel_bias;
102  vff.offset = init_baro_offset;
103  int i, j;
104  for (i=0; i<VFF_STATE_SIZE; i++) {
105  for (j=0; j<VFF_STATE_SIZE; j++)
106  vff.P[i][j] = 0.;
107  vff.P[i][i] = INIT_PXX;
108  }
109 
110 #if PERIODIC_TELEMETRY
111  register_periodic_telemetry(DefaultPeriodic, "VFF_EXTENDED", send_vffe);
112 #endif
113 }
114 
115 
116 /*
117 
118  F = [ 1 dt -dt^2/2 0
119  0 1 -dt 0
120  0 0 1 0
121  0 0 0 1 ];
122 
123  B = [ dt^2/2 dt 0 0]';
124 
125  Q = [ Qzz 0 0 0
126  0 Qzdotzdot 0 0
127  0 0 Qbiasbias 0
128  0 0 0 0 Qoffoff ];
129 
130  Xk1 = F * Xk0 + B * accel;
131 
132  Pk1 = F * Pk0 * F' + Q;
133 
134 */
135 void vff_propagate(float accel) {
136  /* update state */
137  vff.zdotdot = accel + 9.81 - vff.bias;
138  vff.z = vff.z + DT_VFILTER * vff.zdot;
140  /* update covariance */
141  const float FPF00 = vff.P[0][0] + DT_VFILTER * ( vff.P[1][0] + vff.P[0][1] + DT_VFILTER * vff.P[1][1] );
142  const float FPF01 = vff.P[0][1] + DT_VFILTER * ( vff.P[1][1] - vff.P[0][2] - DT_VFILTER * vff.P[1][2] );
143  const float FPF02 = vff.P[0][2] + DT_VFILTER * ( vff.P[1][2] );
144  const float FPF10 = vff.P[1][0] + DT_VFILTER * (-vff.P[2][0] + vff.P[1][1] - DT_VFILTER * vff.P[2][1] );
145  const float FPF11 = vff.P[1][1] + DT_VFILTER * (-vff.P[2][1] - vff.P[1][2] + DT_VFILTER * vff.P[2][2] );
146  const float FPF12 = vff.P[1][2] + DT_VFILTER * (-vff.P[2][2] );
147  const float FPF20 = vff.P[2][0] + DT_VFILTER * ( vff.P[2][1] );
148  const float FPF21 = vff.P[2][1] + DT_VFILTER * (-vff.P[2][2] );
149  const float FPF22 = vff.P[2][2];
150  const float FPF33 = vff.P[3][3];
151 
152  vff.P[0][0] = FPF00 + Qzz;
153  vff.P[0][1] = FPF01;
154  vff.P[0][2] = FPF02;
155  vff.P[1][0] = FPF10;
156  vff.P[1][1] = FPF11 + Qzdotzdot;
157  vff.P[1][2] = FPF12;
158  vff.P[2][0] = FPF20;
159  vff.P[2][1] = FPF21;
160  vff.P[2][2] = FPF22 + Qbiasbias;
161  vff.P[3][3] = FPF33 + Qoffoff;
162 
163 #if DEBUG_VFF_EXTENDED
164  RunOnceEvery(10, send_vffe());
165 #endif
166 }
167 
168 /*
169  * Update sensor "with" offset (baro)
170  H = [1 0 0 -1];
171  // state residual
172  y = rangemeter - H * Xm;
173  // covariance residual
174  S = H*Pm*H' + R;
175  // kalman gain
176  K = Pm*H'*inv(S);
177  // update state
178  Xp = Xm + K*y;
179  // update covariance
180  Pp = Pm - K*H*Pm;
181 */
182 static void update_baro_conf(float z_meas, float conf) {
184 
185  const float y = z_meas - vff.z + vff.offset;
186  const float S = vff.P[0][0] - vff.P[0][3] - vff.P[3][0] + vff.P[3][3] + conf;
187  const float K0 = (vff.P[0][0] - vff.P[0][3]) * 1/S;
188  const float K1 = (vff.P[1][0] - vff.P[1][3]) * 1/S;
189  const float K2 = (vff.P[2][0] - vff.P[2][3]) * 1/S;
190  const float K3 = (vff.P[3][0] - vff.P[3][3]) * 1/S;
191 
192  vff.z = vff.z + K0 * y;
193  vff.zdot = vff.zdot + K1 * y;
194  vff.bias = vff.bias + K2 * y;
195  vff.offset = vff.offset + K3 * y;
196 
197  const float P0 = vff.P[0][0] - vff.P[3][0];
198  const float P1 = vff.P[0][1] - vff.P[3][1];
199  const float P2 = vff.P[0][2] - vff.P[3][2];
200  const float P3 = vff.P[0][3] - vff.P[3][3];
201 
202  vff.P[0][0] -= K0 * P0;
203  vff.P[0][1] -= K0 * P1;
204  vff.P[0][2] -= K0 * P2;
205  vff.P[0][3] -= K0 * P3;
206  vff.P[1][0] -= K1 * P0;
207  vff.P[1][1] -= K1 * P1;
208  vff.P[1][2] -= K1 * P2;
209  vff.P[1][3] -= K1 * P3;
210  vff.P[2][0] -= K2 * P0;
211  vff.P[2][1] -= K2 * P1;
212  vff.P[2][2] -= K2 * P2;
213  vff.P[2][3] -= K2 * P3;
214  vff.P[3][0] -= K3 * P0;
215  vff.P[3][1] -= K3 * P1;
216  vff.P[3][2] -= K3 * P2;
217  vff.P[3][3] -= K3 * P3;
218 
219 }
220 
221 void vff_update_baro(float z_meas) {
222  update_baro_conf(z_meas, R_BARO);
223 }
224 
225 void vff_update_baro_conf(float z_meas, float conf) {
226  update_baro_conf(z_meas, conf);
227 }
228 
229 /*
230  * Update sensor "without" offset (gps, sonar)
231  H = [1 0 0 0];
232  // state residual
233  y = rangemeter - H * Xm;
234  // covariance residual
235  S = H*Pm*H' + R;
236  // kalman gain
237  K = Pm*H'*inv(S);
238  // update state
239  Xp = Xm + K*y;
240  // update covariance
241  Pp = Pm - K*H*Pm;
242 */
243 static void update_alt_conf(float z_meas, float conf) {
244  vff.z_meas = z_meas;
245 
246  const float y = z_meas - vff.z;
247  const float S = vff.P[0][0] + conf;
248  const float K0 = vff.P[0][0] * 1/S;
249  const float K1 = vff.P[1][0] * 1/S;
250  const float K2 = vff.P[2][0] * 1/S;
251  const float K3 = vff.P[3][0] * 1/S;
252 
253  vff.z = vff.z + K0 * y;
254  vff.zdot = vff.zdot + K1 * y;
255  vff.bias = vff.bias + K2 * y;
256  vff.offset = vff.offset + K3 * y;
257 
258  const float P0 = vff.P[0][0];
259  const float P1 = vff.P[0][1];
260  const float P2 = vff.P[0][2];
261  const float P3 = vff.P[0][3];
262 
263  vff.P[0][0] -= K0 * P0;
264  vff.P[0][1] -= K0 * P1;
265  vff.P[0][2] -= K0 * P2;
266  vff.P[0][3] -= K0 * P3;
267  vff.P[1][0] -= K1 * P0;
268  vff.P[1][1] -= K1 * P1;
269  vff.P[1][2] -= K1 * P2;
270  vff.P[1][3] -= K1 * P3;
271  vff.P[2][0] -= K2 * P0;
272  vff.P[2][1] -= K2 * P1;
273  vff.P[2][2] -= K2 * P2;
274  vff.P[2][3] -= K2 * P3;
275  vff.P[3][0] -= K3 * P0;
276  vff.P[3][1] -= K3 * P1;
277  vff.P[3][2] -= K3 * P2;
278  vff.P[3][3] -= K3 * P3;
279 }
280 
281 void vff_update_z(float z_meas) {
282  update_alt_conf(z_meas, R_ALT);
283 }
284 
285 void vff_update_z_conf(float z_meas, float conf) {
286  update_alt_conf(z_meas, conf);
287 }
288 
289 /*
290  * Update sensor offset (baro)
291  H = [0 0 0 1];
292  // state residual
293  y = rangemeter - H * Xm;
294  // covariance residual
295  S = H*Pm*H' + R;
296  // kalman gain
297  K = Pm*H'*inv(S);
298  // update state
299  Xp = Xm + K*y;
300  // update covariance
301  Pp = Pm - K*H*Pm;
302 */
303 static void update_offset_conf(float offset, float conf) {
304 
305  const float y = offset - vff.offset;
306  const float S = vff.P[3][3] + conf;
307  const float K0 = vff.P[0][3] * 1/S;
308  const float K1 = vff.P[1][3] * 1/S;
309  const float K2 = vff.P[2][3] * 1/S;
310  const float K3 = vff.P[3][3] * 1/S;
311 
312  vff.z = vff.z + K0 * y;
313  vff.zdot = vff.zdot + K1 * y;
314  vff.bias = vff.bias + K2 * y;
315  vff.offset = vff.offset + K3 * y;
316 
317  const float P0 = vff.P[3][0];
318  const float P1 = vff.P[3][1];
319  const float P2 = vff.P[3][2];
320  const float P3 = vff.P[3][3];
321 
322  vff.P[0][0] -= K0 * P0;
323  vff.P[0][1] -= K0 * P1;
324  vff.P[0][2] -= K0 * P2;
325  vff.P[0][3] -= K0 * P3;
326  vff.P[1][0] -= K1 * P0;
327  vff.P[1][1] -= K1 * P1;
328  vff.P[1][2] -= K1 * P2;
329  vff.P[1][3] -= K1 * P3;
330  vff.P[2][0] -= K2 * P0;
331  vff.P[2][1] -= K2 * P1;
332  vff.P[2][2] -= K2 * P2;
333  vff.P[2][3] -= K2 * P3;
334  vff.P[3][0] -= K3 * P0;
335  vff.P[3][1] -= K3 * P1;
336  vff.P[3][2] -= K3 * P2;
337  vff.P[3][3] -= K3 * P3;
338 }
339 
341  update_offset_conf(offset, R_OFFSET);
342 }
343 
344 
345 void vff_realign(float z_meas) {
346  //vff.z = z_meas;
347  //vff.zdot = 0.;
348  //vff.offset = 0.;
349  vff_init(z_meas, 0., 0., 0.);
350 }
#define Qzdotzdot
static void update_baro_conf(float z_meas, float conf)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void vff_update_z_conf(float z_meas, float conf)
Interface for extended vertical filter (in float).
float bias
accel bias estimate in m/s^2
Periodic telemetry system header (includes downlink utility and generated code).
#define R_BARO
float zdot
z-velocity estimate in m/s (NED, z-down)
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]
covariance matrix
float z_meas_baro
last z measurement from baro in m
static void update_offset_conf(float offset, float conf)
void vff_update_z(float z_meas)
#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)
float zdotdot
z-acceleration in m/s^2 (NED, z-down)
void vff_init_zero(void)
#define INS_PROPAGATE_FREQUENCY
void vff_realign(float z_meas)
#define Qoffoff
struct VffExtended vff
#define K0
#define R_ALT
#define R_OFFSET
void vff_update_baro(float z_meas)
float z_meas
last z measurement in m
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
float z
z-position estimate in m (NED, z-down)
void vff_propagate(float accel)
#define INIT_PXX
#define DT_VFILTER
float offset
baro offset estimate
#define Qbiasbias
#define DEBUG_VFF_EXTENDED