Paparazzi UAS  v5.12_stable-4-g9b43e9b
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  update_z_conf(z_meas, conf);
197 }
198 
199 /*
200  H = [0 1 0];
201  R = 0.1;
202  // state residual
203  yd = vz - H * Xm;
204  // covariance residual
205  S = H*Pm*H' + R;
206  // kalman gain
207  K = Pm*H'*inv(S);
208  // update state
209  Xp = Xm + K*yd;
210  // update covariance
211  Pp = Pm - K*H*Pm;
212 */
213 static inline void update_vz_conf(float vz, float conf)
214 {
215  const float yd = vz - vff.zdot;
216  const float S = vff.P[1][1] + conf;
217  const float K1 = vff.P[0][1] * 1 / S;
218  const float K2 = vff.P[1][1] * 1 / S;
219  const float K3 = vff.P[2][1] * 1 / S;
220 
221  vff.z = vff.z + K1 * yd;
222  vff.zdot = vff.zdot + K2 * yd;
223  vff.bias = vff.bias + K3 * yd;
224 
225  const float P11 = -K1 * vff.P[1][0] + vff.P[0][0];
226  const float P12 = -K1 * vff.P[1][1] + vff.P[0][1];
227  const float P13 = -K1 * vff.P[1][2] + vff.P[0][2];
228  const float P21 = (1. - K2) * vff.P[1][0];
229  const float P22 = (1. - K2) * vff.P[1][1];
230  const float P23 = (1. - K2) * vff.P[1][2];
231  const float P31 = -K3 * vff.P[1][0] + vff.P[2][0];
232  const float P32 = -K3 * vff.P[1][1] + vff.P[2][1];
233  const float P33 = -K3 * vff.P[1][2] + vff.P[2][2];
234 
235  vff.P[0][0] = P11;
236  vff.P[0][1] = P12;
237  vff.P[0][2] = P13;
238  vff.P[1][0] = P21;
239  vff.P[1][1] = P22;
240  vff.P[1][2] = P23;
241  vff.P[2][0] = P31;
242  vff.P[2][1] = P32;
243  vff.P[2][2] = P33;
244 
245 }
246 
247 void vff_update_vz_conf(float vz_meas, float conf)
248 {
249  update_vz_conf(vz_meas, conf);
250 }
251 
252 void vff_realign(float z_meas)
253 {
254  vff_init(z_meas, 0., 0.);
255 }
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:252
#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:247
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:213