Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
vf_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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 
32 #include "generated/airframe.h"
33 #include "std.h"
34 
36 #ifndef VFF_INIT_PXX
37 #define VFF_INIT_PXX 1.0
38 #endif
39 
41 #ifndef VFF_ACCEL_NOISE
42 #define VFF_ACCEL_NOISE 0.5
43 #endif
44 
46 #ifndef VFF_MEAS_NOISE
47 #define VFF_MEAS_NOISE 1.0
48 #endif
49 
50 /* default parameters */
51 #define Qbiasbias 1e-7
52 
53 struct Vff vff;
54 
55 #if PERIODIC_TELEMETRY
57 
58 static void send_vff(struct transport_tx *trans, struct link_device *dev)
59 {
60  pprz_msg_send_VFF(trans, dev, AC_ID,
61  &vff.z_meas, &vff.z, &vff.zdot, &vff.bias,
62  &vff.P[0][0], &vff.P[1][1], &vff.P[2][2]);
63 }
64 #endif
65 
66 void vff_init_zero(void)
67 {
68  vff_init(0., 0., 0.);
69 }
70 
71 void vff_init(float init_z, float init_zdot, float init_bias)
72 {
73  vff.z = init_z;
74  vff.zdot = init_zdot;
75  vff.bias = init_bias;
76  int i, j;
77  for (i = 0; i < VFF_STATE_SIZE; i++) {
78  for (j = 0; j < VFF_STATE_SIZE; j++) {
79  vff.P[i][j] = 0.;
80  }
81  vff.P[i][i] = VFF_INIT_PXX;
82  }
83 
84 #if PERIODIC_TELEMETRY
86 #endif
87 }
88 
89 
108 void vff_propagate(float accel, float dt)
109 {
110  /* update state (Xk1) */
111  vff.zdotdot = accel + 9.81 - vff.bias;
112  vff.z = vff.z + dt * vff.zdot;
113  vff.zdot = vff.zdot + dt * vff.zdotdot;
114  /* update covariance (Pk1) */
115  const float FPF00 = vff.P[0][0] + dt * (vff.P[1][0] + vff.P[0][1] + dt * vff.P[1][1]);
116  const float FPF01 = vff.P[0][1] + dt * (vff.P[1][1] - vff.P[0][2] - dt * vff.P[1][2]);
117  const float FPF02 = vff.P[0][2] + dt * (vff.P[1][2]);
118  const float FPF10 = vff.P[1][0] + dt * (-vff.P[2][0] + vff.P[1][1] - dt * vff.P[2][1]);
119  const float FPF11 = vff.P[1][1] + dt * (-vff.P[2][1] - vff.P[1][2] + dt * vff.P[2][2]);
120  const float FPF12 = vff.P[1][2] + dt * (-vff.P[2][2]);
121  const float FPF20 = vff.P[2][0] + dt * (vff.P[2][1]);
122  const float FPF21 = vff.P[2][1] + dt * (-vff.P[2][2]);
123  const float FPF22 = vff.P[2][2];
124 
125  vff.P[0][0] = FPF00 + VFF_ACCEL_NOISE * dt * dt / 2.;
126  vff.P[0][1] = FPF01;
127  vff.P[0][2] = FPF02;
128  vff.P[1][0] = FPF10;
129  vff.P[1][1] = FPF11 + VFF_ACCEL_NOISE * dt;
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 
135 }
136 
153 static inline void update_z_conf(float z_meas, float conf)
154 {
155  vff.z_meas = z_meas;
156 
157  const float y = z_meas - vff.z;
158  const float S = vff.P[0][0] + conf;
159  const float K1 = vff.P[0][0] * 1 / S;
160  const float K2 = vff.P[1][0] * 1 / S;
161  const float K3 = vff.P[2][0] * 1 / S;
162 
163  vff.z = vff.z + K1 * y;
164  vff.zdot = vff.zdot + K2 * y;
165  vff.bias = vff.bias + K3 * y;
166 
167  const float P11 = (1. - K1) * vff.P[0][0];
168  const float P12 = (1. - K1) * vff.P[0][1];
169  const float P13 = (1. - K1) * vff.P[0][2];
170  const float P21 = -K2 * vff.P[0][0] + vff.P[1][0];
171  const float P22 = -K2 * vff.P[0][1] + vff.P[1][1];
172  const float P23 = -K2 * vff.P[0][2] + vff.P[1][2];
173  const float P31 = -K3 * vff.P[0][0] + vff.P[2][0];
174  const float P32 = -K3 * vff.P[0][1] + vff.P[2][1];
175  const float P33 = -K3 * vff.P[0][2] + vff.P[2][2];
176 
177  vff.P[0][0] = P11;
178  vff.P[0][1] = P12;
179  vff.P[0][2] = P13;
180  vff.P[1][0] = P21;
181  vff.P[1][1] = P22;
182  vff.P[1][2] = P23;
183  vff.P[2][0] = P31;
184  vff.P[2][1] = P32;
185  vff.P[2][2] = P33;
186 
187 }
188 
189 void vff_update(float z_meas)
190 {
191  update_z_conf(z_meas, VFF_MEAS_NOISE);
192 }
193 
194 void vff_update_z_conf(float z_meas, float conf)
195 {
196  if (conf < 0.f) { return; }
197 
198  update_z_conf(z_meas, conf);
199 }
200 
201 /*
202  H = [0 1 0];
203  R = 0.1;
204  // state residual
205  yd = vz - H * Xm;
206  // covariance residual
207  S = H*Pm*H' + R;
208  // kalman gain
209  K = Pm*H'*inv(S);
210  // update state
211  Xp = Xm + K*yd;
212  // update covariance
213  Pp = Pm - K*H*Pm;
214 */
215 static inline void update_vz_conf(float vz, float conf)
216 {
217  const float yd = vz - vff.zdot;
218  const float S = vff.P[1][1] + conf;
219  const float K1 = vff.P[0][1] * 1 / S;
220  const float K2 = vff.P[1][1] * 1 / S;
221  const float K3 = vff.P[2][1] * 1 / S;
222 
223  vff.z = vff.z + K1 * yd;
224  vff.zdot = vff.zdot + K2 * yd;
225  vff.bias = vff.bias + K3 * yd;
226 
227  const float P11 = -K1 * vff.P[1][0] + vff.P[0][0];
228  const float P12 = -K1 * vff.P[1][1] + vff.P[0][1];
229  const float P13 = -K1 * vff.P[1][2] + vff.P[0][2];
230  const float P21 = (1. - K2) * vff.P[1][0];
231  const float P22 = (1. - K2) * vff.P[1][1];
232  const float P23 = (1. - K2) * vff.P[1][2];
233  const float P31 = -K3 * vff.P[1][0] + vff.P[2][0];
234  const float P32 = -K3 * vff.P[1][1] + vff.P[2][1];
235  const float P33 = -K3 * vff.P[1][2] + vff.P[2][2];
236 
237  vff.P[0][0] = P11;
238  vff.P[0][1] = P12;
239  vff.P[0][2] = P13;
240  vff.P[1][0] = P21;
241  vff.P[1][1] = P22;
242  vff.P[1][2] = P23;
243  vff.P[2][0] = P31;
244  vff.P[2][1] = P32;
245  vff.P[2][2] = P33;
246 
247 }
248 
249 void vff_update_vz_conf(float vz_meas, float conf)
250 {
251  if (conf < 0.f) { return; }
252 
253  update_vz_conf(vz_meas, conf);
254 }
255 
256 void vff_realign(float z_meas)
257 {
258  vff.z = z_meas;
259  vff.zdot = 0.;
260 }
Periodic telemetry system header (includes downlink utility and generated code).
float zdot
z-velocity estimate in m/s (NED, z-down)
Definition: vf_float.h:36
float z
z-position estimate in m (NED, z-down)
Definition: vf_float.h:35
void vff_update_z_conf(float z_meas, float conf)
Definition: vf_float.c:194
struct Vff vff
Definition: vf_float.c:53
void vff_realign(float z_meas)
Definition: vf_float.c:256
#define VFF_MEAS_NOISE
measurement noise covariance R
Definition: vf_float.c:47
float bias
accel bias estimate in m/s^2
Definition: vf_float.h:37
Vertical filter (in float) estimating altitude, velocity and accel bias.
#define VFF_INIT_PXX
initial error covariance diagonal
Definition: vf_float.c:37
#define VFF_STATE_SIZE
static void update_z_conf(float z_meas, float conf)
Update altitude.
Definition: vf_float.c:153
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void vff_init(float init_z, float init_zdot, float init_bias)
Definition: vf_float.c:71
float zdotdot
z-acceleration in m/s^2 (NED, z-down)
Definition: vf_float.h:38
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
void vff_update_vz_conf(float vz_meas, float conf)
Definition: vf_float.c:249
void vff_update(float z_meas)
Definition: vf_float.c:189
void vff_init_zero(void)
Definition: vf_float.c:66
float z_meas
last measurement
Definition: vf_float.h:39
#define Qbiasbias
Definition: vf_float.c:51
static void send_vff(struct transport_tx *trans, struct link_device *dev)
Definition: vf_float.c:58
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]
covariance matrix
Definition: vf_float.h:40
#define VFF_ACCEL_NOISE
process noise covariance Q
Definition: vf_float.c:42
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Definition: vf_float.h:34
void vff_propagate(float accel, float dt)
Propagate the filter in time.
Definition: vf_float.c:108
static void update_vz_conf(float vz, float conf)
Definition: vf_float.c:215