Paparazzi UAS  v5.15_devel-46-gd23ed71
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.0f
53 #endif
54 
56 #ifndef VFF_EXTENDED_ACCEL_NOISE
57 #define VFF_EXTENDED_ACCEL_NOISE 0.5f
58 #endif
59 
61 #ifndef VFF_EXTENDED_R_BARO
62 #define VFF_EXTENDED_R_BARO 2.f
63 #endif
64 
65 #define Qbiasbias 1e-6
66 
77 #ifdef VFF_EXTENDED_NON_FLAT_GROUND
78 #define Qoffoff 1e-6f
79 // higher value of Qobsobs results in more reliance on baro but smaller jump when flying over obstacle
80 #define Qobsobs 1e-3f
81 #else
82 #define Qoffoff 1e-4f
83 #define Qobsobs 0.f
84 #endif
85 
86 #define R_ALT 0.2f
87 #define R_OBS_HEIGHT 8.f
88 
90 
92 
93 #if PERIODIC_TELEMETRY
95 
96 static void send_vffe(struct transport_tx *trans, struct link_device *dev)
97 {
98  pprz_msg_send_VFF_EXTENDED(trans, dev, AC_ID,
100  &vff.z, &vff.zdot, &vff.zdotdot,
102  &vff.P[0][0], &vff.P[1][1], &vff.P[2][2],
103  &vff.P[3][3], &vff.P[4][4]);
104 }
105 #endif
106 
107 void vff_init_zero(void)
108 {
109  vff_init(0.f, 0.f, 0.f, 0.f, 0.f);
110 }
111 
112 #if DEBUG_VFF_EXTENDED > 1
113 #include "stdio.h"
114 static void print_vff(void)
115 {
116  int i, j;
117  for (i = 0; i < VFF_STATE_SIZE; i++) {
118  for (j = 0; j < VFF_STATE_SIZE; j++) {
119  printf("%f ", vff.P[i][j]);
120  }
121  printf("\n");
122  }
123  printf("\n");
124 }
125 #endif
126 
127 void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_offset, float init_obs_height)
128 {
129  vff.z = init_z;
130  vff.zdot = init_zdot;
131  vff.bias = init_accel_bias;
132  vff.offset = init_offset;
133  vff.obs_height = init_obs_height;
134  int i, j;
135  for (i = 0; i < VFF_STATE_SIZE; i++) {
136  for (j = 0; j < VFF_STATE_SIZE; j++) {
137  vff.P[i][j] = 0.f;
138  }
139  vff.P[i][i] = VFF_EXTENDED_INIT_PXX;
140  }
141 
142 #ifndef VFF_EXTENDED_NON_FLAT_GROUND
143  vff.P[4][4] = 0.f;
144 #endif
145 
148  vff.r_alt = R_ALT;
150 
151 #if PERIODIC_TELEMETRY
152  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VFF_EXTENDED, send_vffe);
153 #endif
154 }
155 
156 
181 void vff_propagate(float accel, float dt)
182 {
183  /* update state */
184  vff.zdotdot = accel + 9.81f - vff.bias;
185  vff.z += (vff.zdot + vff.zdotdot * dt) * dt / 2.f; // trapizium integration
186  vff.zdot += vff.zdotdot * dt;
187 
188  /* update covariance */
189  const float FPF00 = vff.P[0][0] + dt * (vff.P[1][0] + vff.P[0][1] + dt * vff.P[1][1]);
190  const float FPF01 = vff.P[0][1] + dt * (vff.P[1][1] - vff.P[0][2] - dt * vff.P[1][2]);
191  const float FPF02 = vff.P[0][2] + dt * (vff.P[1][2]);
192  const float FPF10 = vff.P[1][0] + dt * (-vff.P[2][0] + vff.P[1][1] - dt * vff.P[2][1]);
193  const float FPF11 = vff.P[1][1] + dt * (-vff.P[2][1] - vff.P[1][2] + dt * vff.P[2][2]);
194  const float FPF12 = vff.P[1][2] + dt * (-vff.P[2][2]);
195  const float FPF20 = vff.P[2][0] + dt * (vff.P[2][1]);
196  const float FPF21 = vff.P[2][1] + dt * (-vff.P[2][2]);
197  const float FPF22 = vff.P[2][2];
198  const float FPF33 = vff.P[3][3];
199  const float FPF44 = vff.P[4][4];
200 
201  vff.P[0][0] = FPF00 + vff.accel_noise * dt * dt / 2.f;
202  vff.P[0][1] = FPF01;
203  vff.P[0][2] = FPF02;
204  vff.P[1][0] = FPF10;
205  vff.P[1][1] = FPF11 + vff.accel_noise * dt;
206  vff.P[1][2] = FPF12;
207  vff.P[2][0] = FPF20;
208  vff.P[2][1] = FPF21;
209  vff.P[2][2] = FPF22 + Qbiasbias;
210  vff.P[3][3] = FPF33 + Qoffoff;
211  vff.P[4][4] = FPF44 + Qobsobs;
212 
213 #if DEBUG_VFF_EXTENDED
214  RunOnceEvery(10, send_vffe(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
215 #endif
216 #if DEBUG_VFF_EXTENDED > 1
217  RunOnceEvery(100, print_vff());
218 #endif
219 }
220 
236 static void update_biased_z_conf(float z_meas, float conf)
237 {
238  static float K[VFF_STATE_SIZE];
239  static float P[VFF_STATE_SIZE];
240 
242 
243  const float y = z_meas - vff.z + vff.offset;
244  const float S = vff.P[0][0] - vff.P[0][3] - vff.P[3][0] + vff.P[3][3] + conf;
245 
246  int i, j;
247  for (i = 0; i < VFF_STATE_SIZE; i++) {
248  K[i] = (vff.P[i][0] - vff.P[i][3]) / S;
249  P[i] = vff.P[0][i] - vff.P[3][i];
250  }
251 
252  vff.z += K[0] * y;
253  vff.zdot += K[1] * y;
254  vff.bias += K[2] * y;
255  vff.offset += K[3] * y;
256  vff.obs_height += K[4] * y;
257 
258  for (i = 0; i < VFF_STATE_SIZE; i++) {
259  for (j = 0; j < VFF_STATE_SIZE; j++) {
260  vff.P[i][j] -= K[i] * P[j];
261  }
262  }
263 
265 }
266 
281 static void update_alt_conf(float z_meas, float conf)
282 {
283  static float K[VFF_STATE_SIZE];
284  static float P[VFF_STATE_SIZE];
285 
286  vff.z_meas = z_meas;
287 
288  const float y = z_meas - vff.z + vff.obs_height;
289  const float S = vff.P[0][0] - vff.P[0][4] - vff.P[4][0] + vff.P[4][4] + conf;
290 
291  int i, j;
292  for (i = 0; i < VFF_STATE_SIZE; i++) {
293  K[i] = (vff.P[i][0] - vff.P[i][4]) / S;
294  P[i] = vff.P[0][i] - vff.P[4][i];
295  }
296 
297  vff.z += K[0] * y;
298  vff.zdot += K[1] * y;
299  vff.bias += K[2] * y;
300  vff.offset += K[3] * y;
301  vff.obs_height += K[4] * y;
302 
303  for (i = 0; i < VFF_STATE_SIZE; i++) {
304  for (j = 0; j < VFF_STATE_SIZE; j++) {
305  vff.P[i][j] -= K[i] * P[j];
306  }
307  }
308 }
309 
310 void vff_update_z_conf(float z_meas, float conf)
311 {
312  if (conf < 0.f) { return; }
313  update_alt_conf(z_meas, conf);
314 }
315 
316 void vff_update_z(float z_meas)
317 {
318  vff_update_z_conf(z_meas, vff.r_alt);
319 }
320 
321 void vff_update_agl(float z_meas, float conf)
322 {
323  if (conf < 0.f) { return; }
324  vff_update_z_conf(z_meas, conf);
325 }
326 
327 void vff_update_baro_conf(float z_meas, float conf)
328 {
329  if (conf < 0.f) { return; }
330  update_biased_z_conf(z_meas, conf);
331 }
332 
334 {
336 }
337 
352 static void update_obs_height(float obs_height, float conf)
353 {
354  static float K[VFF_STATE_SIZE];
355  static float P[VFF_STATE_SIZE];
356 
357  const float y = obs_height - vff.obs_height;
358  const float S = vff.P[4][4] + conf;
359 
360  int i, j;
361  for (i = 0; i < VFF_STATE_SIZE; i++) {
362  K[i] = vff.P[i][4] / S;
363  P[i] = vff.P[4][i];
364  }
365 
366  vff.z += K[0] * y;
367  vff.zdot += K[1] * y;
368  vff.bias += K[2] * y;
369  vff.offset += K[3] * y;
370  vff.obs_height += K[4] * y;
371 
372  for (i = 0; i < VFF_STATE_SIZE; i++) {
373  for (j = 0; j < VFF_STATE_SIZE; j++) {
374  vff.P[i][j] -= K[i] * P[j];
375  }
376  }
377 }
378 
380 {
381  update_obs_height(obs_height, vff.r_obs_height);
382 }
383 
384 void vff_realign(float z_meas)
385 {
386  vff.z = z_meas;
387  vff.zdot = 0.f;
388  vff.offset = 0.f;
389  vff.obs_height = 0.f;
390 }
391 
392 /*
393  H = [0 1 0 0 0];
394  R = 0.1;
395  // state residual
396  yd = vzd - H * Xm;
397  // covariance residual
398  S = H*Pm*H' + R;
399  // kalman gain
400  K = Pm*H'*inv(S);
401  // update state
402  Xp = Xm + K*yd;
403  // update covariance
404  Pp = Pm - K*H*Pm;
405 */
406 static inline void update_vz_conf(float vz, float conf)
407 {
408  static float K[VFF_STATE_SIZE];
409  static float P[VFF_STATE_SIZE];
410 
411  const float yd = vz - vff.zdot;
412  const float S = vff.P[1][1] + conf;
413 
414  int i, j;
415  for (i = 0; i < VFF_STATE_SIZE; i++) {
416  K[i] = vff.P[i][1] * 1 / S;
417  P[i] = vff.P[1][i];
418  }
419 
420  vff.z += K[0] * yd;
421  vff.zdot += K[1] * yd;
422  vff.bias += K[2] * yd;
423  vff.offset += K[3] * yd;
424  vff.obs_height += K[4] * yd;
425 
426  for (i = 0; i < VFF_STATE_SIZE; i++) {
427  for (j = 0; j < VFF_STATE_SIZE; j++) {
428  vff.P[i][j] -= K[i] * P[j];
429  }
430  }
431 }
432 
433 void vff_update_vz_conf(float vz_meas, float conf)
434 {
435  if (conf < 0.f) { return; }
436 
437  update_vz_conf(vz_meas, conf);
438 }
#define VFF_EXTENDED_ACCEL_NOISE
process noise covariance Q
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
void vff_update_z_conf(float z_meas, float conf)
Interface for extended vertical filter (in float).
static float K[9]
Periodic telemetry system header (includes downlink utility and generated code).
float bias
accel bias estimate in m/s^2
void vff_update_vz_conf(float vz_meas, float conf)
static float P[3][3]
Definition: trilateration.c:31
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_offset, float init_obs_height)
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)
#define VFF_EXTENDED_R_BARO
Barometer confidence.
static void update_obs_height(float obs_height, float conf)
Update obstacle height.
void vff_update_z(float z_meas)
#define Qobsobs
#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 -1]; // state residual y = rangemeter - H * ...
float offset
baro offset estimate
#define R_OBS_HEIGHT
#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_update_agl(float z_meas, float conf)
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)
float obs_height
estimate of height of obstacles under the vehicle
#define Qoffoff
VFF_EXTENDED_NON_FLAT_GROUND removes the assumption of a flat ground and tries to estimate the height...
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
static void update_biased_z_conf(float z_meas, float conf)
Update sensor "with" offset (baro, sonar).
void vff_update_baro(float z_meas)
float z_meas_baro
last z measurement from baro in m
void vff_update_obs_height(float obs_height)
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