Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 
35 #include "generated/airframe.h"
36 #include "std.h"
37 
38 #ifndef DEBUG_VFF_EXTENDED
39 #define DEBUG_VFF_EXTENDED 0
40 #else
41 PRINT_CONFIG_VAR(DEBUG_VFF_EXTENDED)
42 #endif
43 
44 #if DEBUG_VFF_EXTENDED
45 #include "mcu_periph/uart.h"
46 #include "pprzlink/messages.h"
48 #endif
49 
51 #ifndef VFF_EXTENDED_INIT_PXX
52 #define VFF_EXTENDED_INIT_PXX 1.0
53 #endif
54 
56 #ifndef VFF_EXTENDED_ACCEL_NOISE
57 #define VFF_EXTENDED_ACCEL_NOISE 0.5
58 #endif
59 
60 #define Qbiasbias 1e-7
61 #define Qoffoff 1e-4
62 #define R_BARO 1.
63 #define R_ALT 0.1
64 #define R_OFFSET 1.
65 
67 
68 #if PERIODIC_TELEMETRY
70 
71 static void send_vffe(struct transport_tx *trans, struct link_device *dev)
72 {
73  pprz_msg_send_VFF_EXTENDED(trans, dev, AC_ID,
75  &vff.z, &vff.zdot, &vff.zdotdot,
76  &vff.bias, &vff.offset);
77 }
78 #endif
79 
80 void vff_init_zero(void)
81 {
82  vff_init(0., 0., 0., 0.);
83 }
84 
85 void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
86 {
87  vff.z = init_z;
88  vff.zdot = init_zdot;
89  vff.bias = init_accel_bias;
90  vff.offset = init_baro_offset;
91  int i, j;
92  for (i = 0; i < VFF_STATE_SIZE; i++) {
93  for (j = 0; j < VFF_STATE_SIZE; j++) {
94  vff.P[i][j] = 0.;
95  }
96  vff.P[i][i] = VFF_EXTENDED_INIT_PXX;
97  }
98 
99 #if PERIODIC_TELEMETRY
100  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VFF_EXTENDED, send_vffe);
101 #endif
102 }
103 
104 
127 void vff_propagate(float accel, float dt)
128 {
129  /* update state */
130  vff.zdotdot = accel + 9.81 - vff.bias;
131  vff.z = vff.z + dt * vff.zdot;
132  vff.zdot = vff.zdot + dt * vff.zdotdot;
133  /* update covariance */
134  const float FPF00 = vff.P[0][0] + dt * (vff.P[1][0] + vff.P[0][1] + dt * vff.P[1][1]);
135  const float FPF01 = vff.P[0][1] + dt * (vff.P[1][1] - vff.P[0][2] - dt * vff.P[1][2]);
136  const float FPF02 = vff.P[0][2] + dt * (vff.P[1][2]);
137  const float FPF10 = vff.P[1][0] + dt * (-vff.P[2][0] + vff.P[1][1] - dt * vff.P[2][1]);
138  const float FPF11 = vff.P[1][1] + dt * (-vff.P[2][1] - vff.P[1][2] + dt * vff.P[2][2]);
139  const float FPF12 = vff.P[1][2] + dt * (-vff.P[2][2]);
140  const float FPF20 = vff.P[2][0] + dt * (vff.P[2][1]);
141  const float FPF21 = vff.P[2][1] + dt * (-vff.P[2][2]);
142  const float FPF22 = vff.P[2][2];
143  const float FPF33 = vff.P[3][3];
144 
145  vff.P[0][0] = FPF00 + VFF_EXTENDED_ACCEL_NOISE * dt * dt / 2.;
146  vff.P[0][1] = FPF01;
147  vff.P[0][2] = FPF02;
148  vff.P[1][0] = FPF10;
149  vff.P[1][1] = FPF11 + VFF_EXTENDED_ACCEL_NOISE * dt;
150  vff.P[1][2] = FPF12;
151  vff.P[2][0] = FPF20;
152  vff.P[2][1] = FPF21;
153  vff.P[2][2] = FPF22 + Qbiasbias;
154  vff.P[3][3] = FPF33 + Qoffoff;
155 
156 #if DEBUG_VFF_EXTENDED
157  RunOnceEvery(10, send_vffe(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
158 #endif
159 }
160 
176 static void update_baro_conf(float z_meas, float conf)
177 {
179 
180  const float y = z_meas - vff.z + vff.offset;
181  const float S = vff.P[0][0] - vff.P[0][3] - vff.P[3][0] + vff.P[3][3] + conf;
182  const float K0 = (vff.P[0][0] - vff.P[0][3]) * 1 / S;
183  const float K1 = (vff.P[1][0] - vff.P[1][3]) * 1 / S;
184  const float K2 = (vff.P[2][0] - vff.P[2][3]) * 1 / S;
185  const float K3 = (vff.P[3][0] - vff.P[3][3]) * 1 / S;
186 
187  vff.z = vff.z + K0 * y;
188  vff.zdot = vff.zdot + K1 * y;
189  vff.bias = vff.bias + K2 * y;
190  vff.offset = vff.offset + K3 * y;
191 
192  const float P0 = vff.P[0][0] - vff.P[3][0];
193  const float P1 = vff.P[0][1] - vff.P[3][1];
194  const float P2 = vff.P[0][2] - vff.P[3][2];
195  const float P3 = vff.P[0][3] - vff.P[3][3];
196 
197  vff.P[0][0] -= K0 * P0;
198  vff.P[0][1] -= K0 * P1;
199  vff.P[0][2] -= K0 * P2;
200  vff.P[0][3] -= K0 * P3;
201  vff.P[1][0] -= K1 * P0;
202  vff.P[1][1] -= K1 * P1;
203  vff.P[1][2] -= K1 * P2;
204  vff.P[1][3] -= K1 * P3;
205  vff.P[2][0] -= K2 * P0;
206  vff.P[2][1] -= K2 * P1;
207  vff.P[2][2] -= K2 * P2;
208  vff.P[2][3] -= K2 * P3;
209  vff.P[3][0] -= K3 * P0;
210  vff.P[3][1] -= K3 * P1;
211  vff.P[3][2] -= K3 * P2;
212  vff.P[3][3] -= K3 * P3;
213 
214 }
215 
217 {
218  update_baro_conf(z_meas, R_BARO);
219 }
220 
221 void vff_update_baro_conf(float z_meas, float conf)
222 {
223  update_baro_conf(z_meas, conf);
224 }
225 
240 static void update_alt_conf(float z_meas, float conf)
241 {
242  vff.z_meas = z_meas;
243 
244  const float y = z_meas - vff.z;
245  const float S = vff.P[0][0] + conf;
246  const float K0 = vff.P[0][0] * 1 / S;
247  const float K1 = vff.P[1][0] * 1 / S;
248  const float K2 = vff.P[2][0] * 1 / S;
249  const float K3 = vff.P[3][0] * 1 / S;
250 
251  vff.z = vff.z + K0 * y;
252  vff.zdot = vff.zdot + K1 * y;
253  vff.bias = vff.bias + K2 * y;
254  vff.offset = vff.offset + K3 * y;
255 
256  const float P0 = vff.P[0][0];
257  const float P1 = vff.P[0][1];
258  const float P2 = vff.P[0][2];
259  const float P3 = vff.P[0][3];
260 
261  vff.P[0][0] -= K0 * P0;
262  vff.P[0][1] -= K0 * P1;
263  vff.P[0][2] -= K0 * P2;
264  vff.P[0][3] -= K0 * P3;
265  vff.P[1][0] -= K1 * P0;
266  vff.P[1][1] -= K1 * P1;
267  vff.P[1][2] -= K1 * P2;
268  vff.P[1][3] -= K1 * P3;
269  vff.P[2][0] -= K2 * P0;
270  vff.P[2][1] -= K2 * P1;
271  vff.P[2][2] -= K2 * P2;
272  vff.P[2][3] -= K2 * P3;
273  vff.P[3][0] -= K3 * P0;
274  vff.P[3][1] -= K3 * P1;
275  vff.P[3][2] -= K3 * P2;
276  vff.P[3][3] -= K3 * P3;
277 }
278 
279 void vff_update_z(float z_meas)
280 {
281  update_alt_conf(z_meas, R_ALT);
282 }
283 
284 void vff_update_z_conf(float z_meas, float conf)
285 {
286  update_alt_conf(z_meas, conf);
287 }
288 
303 static void update_offset_conf(float offset, float conf)
304 {
305 
306  const float y = offset - vff.offset;
307  const float S = vff.P[3][3] + conf;
308  const float K0 = vff.P[0][3] * 1 / S;
309  const float K1 = vff.P[1][3] * 1 / S;
310  const float K2 = vff.P[2][3] * 1 / S;
311  const float K3 = vff.P[3][3] * 1 / S;
312 
313  vff.z = vff.z + K0 * y;
314  vff.zdot = vff.zdot + K1 * y;
315  vff.bias = vff.bias + K2 * y;
316  vff.offset = vff.offset + K3 * y;
317 
318  const float P0 = vff.P[3][0];
319  const float P1 = vff.P[3][1];
320  const float P2 = vff.P[3][2];
321  const float P3 = vff.P[3][3];
322 
323  vff.P[0][0] -= K0 * P0;
324  vff.P[0][1] -= K0 * P1;
325  vff.P[0][2] -= K0 * P2;
326  vff.P[0][3] -= K0 * P3;
327  vff.P[1][0] -= K1 * P0;
328  vff.P[1][1] -= K1 * P1;
329  vff.P[1][2] -= K1 * P2;
330  vff.P[1][3] -= K1 * P3;
331  vff.P[2][0] -= K2 * P0;
332  vff.P[2][1] -= K2 * P1;
333  vff.P[2][2] -= K2 * P2;
334  vff.P[2][3] -= K2 * P3;
335  vff.P[3][0] -= K3 * P0;
336  vff.P[3][1] -= K3 * P1;
337  vff.P[3][2] -= K3 * P2;
338  vff.P[3][3] -= K3 * P3;
339 }
340 
342 {
343  update_offset_conf(offset, R_OFFSET);
344 }
345 
346 
347 void vff_realign(float z_meas)
348 {
349  //vff.z = z_meas;
350  //vff.zdot = 0.;
351  //vff.offset = 0.;
352  vff_init(z_meas, 0., 0., 0.);
353 }
354 
355 /*
356  H = [0 1 0 0];
357  R = 0.1;
358  // state residual
359  yd = vzd - H * Xm;
360  // covariance residual
361  S = H*Pm*H' + R;
362  // kalman gain
363  K = Pm*H'*inv(S);
364  // update state
365  Xp = Xm + K*yd;
366  // update covariance
367  Pp = Pm - K*H*Pm;
368 */
369 static inline void update_vz_conf(float vz, float conf)
370 {
371  const float yd = vz - vff.zdot;
372  const float S = vff.P[1][1] + conf;
373  const float K0 = vff.P[0][1] * 1 / S;
374  const float K1 = vff.P[1][1] * 1 / S;
375  const float K2 = vff.P[2][1] * 1 / S;
376  const float K3 = vff.P[3][1] * 1 / S;
377 
378  vff.z = vff.z + K0 * yd;
379  vff.zdot = vff.zdot + K1 * yd;
380  vff.bias = vff.bias + K2 * yd;
381  vff.offset = vff.offset + K3 * yd;
382 
383  const float P0 = vff.P[1][0];
384  const float P1 = vff.P[1][1];
385  const float P2 = vff.P[1][2];
386  const float P3 = vff.P[1][3];
387 
388  vff.P[0][0] -= K0 * P0;
389  vff.P[0][1] -= K0 * P1;
390  vff.P[0][2] -= K0 * P2;
391  vff.P[0][3] -= K0 * P3;
392  vff.P[1][0] -= K1 * P0;
393  vff.P[1][1] -= K1 * P1;
394  vff.P[1][2] -= K1 * P2;
395  vff.P[1][3] -= K1 * P3;
396  vff.P[2][0] -= K2 * P0;
397  vff.P[2][1] -= K2 * P1;
398  vff.P[2][2] -= K2 * P2;
399  vff.P[2][3] -= K2 * P3;
400  vff.P[3][0] -= K3 * P0;
401  vff.P[3][1] -= K3 * P1;
402  vff.P[3][2] -= K3 * P2;
403  vff.P[3][3] -= K3 * P3;
404 }
405 
406 void vff_update_vz_conf(float vz_meas, float conf)
407 {
408  update_vz_conf(vz_meas, conf);
409 }
#define VFF_EXTENDED_ACCEL_NOISE
process noise covariance Q
static void update_baro_conf(float z_meas, float conf)
Update sensor "with" offset (baro).
#define K0
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define VFF_EXTENDED_INIT_PXX
initial covariance diagonal
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]
covariance matrix
static struct EcefCoor_d offset
void vff_update_z_conf(float z_meas, float conf)
Interface for extended vertical filter (in float).
Periodic telemetry system header (includes downlink utility and generated code).
#define R_BARO
float bias
accel bias estimate in m/s^2
void vff_update_vz_conf(float vz_meas, float conf)
float dt
float z
z-position estimate in m (NED, z-down)
static void update_vz_conf(float vz, float conf)
static void send_vffe(struct transport_tx *trans, struct link_device *dev)
static void update_offset_conf(float offset, float conf)
Update sensor offset (baro).
void vff_update_z(float z_meas)
#define VFF_STATE_SIZE
void vff_update_baro_conf(float z_meas, float conf)
static void update_alt_conf(float z_meas, float conf)
Update sensor "without" offset (gps, sonar) H = [1 0 0 0]; // state residual y = rangemeter - H * Xm;...
float offset
baro offset estimate
void vff_update_offset(float offset)
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void vff_init_zero(void)
float zdotdot
z-acceleration in m/s^2 (NED, z-down)
void vff_propagate(float accel, float dt)
Propagate the filter in time.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
void vff_realign(float z_meas)
#define Qoffoff
float z_meas
last z measurement in m
struct VffExtended vff
float zdot
z-velocity estimate in m/s (NED, z-down)
#define R_ALT
#define R_OFFSET
void vff_update_baro(float z_meas)
float z_meas_baro
last z measurement from baro in m
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
#define Qbiasbias
#define DEBUG_VFF_EXTENDED