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_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 
30 #include "generated/airframe.h"
31 #include "std.h"
32 
33 #ifndef INS_PROPAGATE_FREQUENCY
34 #ifdef AHRS_PROPAGATE_FREQUENCY
35 #define INS_PROPAGATE_FREQUENCY AHRS_PROPAGATE_FREQUENCY
36 #else
37 #define INS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
38 #endif
39 #endif
41 
42 #define DT_VFILTER (1./(INS_PROPAGATE_FREQUENCY))
43 
44 /*
45 
46 X = [ z zdot bias ]
47 
48 temps :
49  propagate 86us
50  update 46us
51 
52 */
53 /* initial error covariance diagonal */
54 #ifndef VF_FLOAT_INIT_PXX
55 #define VF_FLOAT_INIT_PXX 1.0
56 #endif
57 
58 /* process noise covariance Q */
59 #ifndef VF_FLOAT_ACCEL_NOISE
60 #define VF_FLOAT_ACCEL_NOISE 0.5
61 #endif
62 
63 /* measurement noise covariance R */
64 #ifndef VF_FLOAT_MEAS_NOISE
65 #define VF_FLOAT_MEAS_NOISE 1.0
66 #endif
67 
68 /* default parameters */
69 #define Qzz VF_FLOAT_ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
70 #define Qzdotzdot VF_FLOAT_ACCEL_NOISE * DT_VFILTER
71 #define Qbiasbias 1e-7
72 
73 struct Vff vff;
74 
75 #if PERIODIC_TELEMETRY
77 
78 static void send_vff(void) {
79  DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice,
80  &vff.z_meas, &vff.z, &vff.zdot, &vff.bias,
81  &vff.P[0][0], &vff.P[1][1], &vff.P[2][2]);
82 }
83 #endif
84 
85 void vff_init_zero(void) {
86  vff_init(0., 0., 0.);
87 }
88 
89 void vff_init(float init_z, float init_zdot, float init_bias) {
90  vff.z = init_z;
91  vff.zdot = init_zdot;
92  vff.bias = init_bias;
93  int i, j;
94  for (i=0; i<VFF_STATE_SIZE; i++) {
95  for (j=0; j<VFF_STATE_SIZE; j++)
96  vff.P[i][j] = 0.;
97  vff.P[i][i] = VF_FLOAT_INIT_PXX;
98  }
99 
100 #if PERIODIC_TELEMETRY
101  register_periodic_telemetry(DefaultPeriodic, "VFF", send_vff);
102 #endif
103 }
104 
105 
106 /*
107 
108  F = [ 1 dt -dt^2/2
109  0 1 -dt
110  0 0 1 ];
111 
112  B = [ dt^2/2 dt 0]';
113 
114  Q = [ 0.01 0 0
115  0 0.01 0
116  0 0 0.001 ];
117 
118  Xk1 = F * Xk0 + B * accel;
119 
120  Pk1 = F * Pk0 * F' + Q;
121 
122 */
123 void vff_propagate(float accel) {
124  /* update state (Xk1) */
125  vff.zdotdot = accel + 9.81 - vff.bias;
126  vff.z = vff.z + DT_VFILTER * vff.zdot;
128  /* update covariance (Pk1) */
129  const float FPF00 = vff.P[0][0] + DT_VFILTER * ( vff.P[1][0] + vff.P[0][1] + DT_VFILTER * vff.P[1][1] );
130  const float FPF01 = vff.P[0][1] + DT_VFILTER * ( vff.P[1][1] - vff.P[0][2] - DT_VFILTER * vff.P[1][2] );
131  const float FPF02 = vff.P[0][2] + DT_VFILTER * ( vff.P[1][2] );
132  const float FPF10 = vff.P[1][0] + DT_VFILTER * (-vff.P[2][0] + vff.P[1][1] - DT_VFILTER * vff.P[2][1] );
133  const float FPF11 = vff.P[1][1] + DT_VFILTER * (-vff.P[2][1] - vff.P[1][2] + DT_VFILTER * vff.P[2][2] );
134  const float FPF12 = vff.P[1][2] + DT_VFILTER * (-vff.P[2][2] );
135  const float FPF20 = vff.P[2][0] + DT_VFILTER * ( vff.P[2][1] );
136  const float FPF21 = vff.P[2][1] + DT_VFILTER * (-vff.P[2][2] );
137  const float FPF22 = vff.P[2][2];
138 
139  vff.P[0][0] = FPF00 + Qzz;
140  vff.P[0][1] = FPF01;
141  vff.P[0][2] = FPF02;
142  vff.P[1][0] = FPF10;
143  vff.P[1][1] = FPF11 + Qzdotzdot;
144  vff.P[1][2] = FPF12;
145  vff.P[2][0] = FPF20;
146  vff.P[2][1] = FPF21;
147  vff.P[2][2] = FPF22 + Qbiasbias;
148 
149 }
150 /*
151  H = [1 0 0];
152  R = 0.1;
153  // state residual
154  y = rangemeter - H * Xm;
155  // covariance residual
156  S = H*Pm*H' + R;
157  // kalman gain
158  K = Pm*H'*inv(S);
159  // update state
160  Xp = Xm + K*y;
161  // update covariance
162  Pp = Pm - K*H*Pm;
163 */
164 static inline void update_z_conf(float z_meas, float conf) {
165  vff.z_meas = z_meas;
166 
167  const float y = z_meas - vff.z;
168  const float S = vff.P[0][0] + conf;
169  const float K1 = vff.P[0][0] * 1/S;
170  const float K2 = vff.P[1][0] * 1/S;
171  const float K3 = vff.P[2][0] * 1/S;
172 
173  vff.z = vff.z + K1 * y;
174  vff.zdot = vff.zdot + K2 * y;
175  vff.bias = vff.bias + K3 * y;
176 
177  const float P11 = (1. - K1) * vff.P[0][0];
178  const float P12 = (1. - K1) * vff.P[0][1];
179  const float P13 = (1. - K1) * vff.P[0][2];
180  const float P21 = -K2 * vff.P[0][0] + vff.P[1][0];
181  const float P22 = -K2 * vff.P[0][1] + vff.P[1][1];
182  const float P23 = -K2 * vff.P[0][2] + vff.P[1][2];
183  const float P31 = -K3 * vff.P[0][0] + vff.P[2][0];
184  const float P32 = -K3 * vff.P[0][1] + vff.P[2][1];
185  const float P33 = -K3 * vff.P[0][2] + vff.P[2][2];
186 
187  vff.P[0][0] = P11;
188  vff.P[0][1] = P12;
189  vff.P[0][2] = P13;
190  vff.P[1][0] = P21;
191  vff.P[1][1] = P22;
192  vff.P[1][2] = P23;
193  vff.P[2][0] = P31;
194  vff.P[2][1] = P32;
195  vff.P[2][2] = P33;
196 
197 }
198 
199 void vff_update(float z_meas) {
201 }
202 
203 void vff_update_z_conf(float z_meas, float conf) {
204  update_z_conf(z_meas, conf);
205 }
206 
207 /*
208  H = [0 1 0];
209  R = 0.1;
210  // state residual
211  yd = vz - H * Xm;
212  // covariance residual
213  S = H*Pm*H' + R;
214  // kalman gain
215  K = Pm*H'*inv(S);
216  // update state
217  Xp = Xm + K*yd;
218  // update covariance
219  Pp = Pm - K*H*Pm;
220 */
221 static inline void update_vz_conf(float vz, float conf) {
222  const float yd = vz - vff.zdot;
223  const float S = vff.P[1][1] + conf;
224  const float K1 = vff.P[0][1] * 1/S;
225  const float K2 = vff.P[1][1] * 1/S;
226  const float K3 = vff.P[2][1] * 1/S;
227 
228  vff.z = vff.z + K1 * yd;
229  vff.zdot = vff.zdot + K2 * yd;
230  vff.bias = vff.bias + K3 * yd;
231 
232  const float P11 = -K1 * vff.P[1][0] + vff.P[0][0];
233  const float P12 = -K1 * vff.P[1][1] + vff.P[0][1];
234  const float P13 = -K1 * vff.P[1][2] + vff.P[0][2];
235  const float P21 = (1. - K2) * vff.P[1][0];
236  const float P22 = (1. - K2) * vff.P[1][1];
237  const float P23 = (1. - K2) * vff.P[1][2];
238  const float P31 = -K3 * vff.P[1][0] + vff.P[2][0];
239  const float P32 = -K3 * vff.P[1][1] + vff.P[2][1];
240  const float P33 = -K3 * vff.P[1][2] + vff.P[2][2];
241 
242  vff.P[0][0] = P11;
243  vff.P[0][1] = P12;
244  vff.P[0][2] = P13;
245  vff.P[1][0] = P21;
246  vff.P[1][1] = P22;
247  vff.P[1][2] = P23;
248  vff.P[2][0] = P31;
249  vff.P[2][1] = P32;
250  vff.P[2][2] = P33;
251 
252 }
253 
254 void vff_update_vz_conf(float vz_meas, float conf) {
255  update_vz_conf(vz_meas, conf);
256 }
257 
258 void vff_realign(float z_meas) {
259  vff.z = z_meas;
260  vff.zdot = 0;
261 }
Definition: vf_float.h:34
void vff_propagate(float accel)
Definition: vf_float.c:123
float zdotdot
z-acceleration in m/s^2 (NED, z-down)
Definition: vf_float.h:38
#define Qzz
Definition: vf_float.c:69
Periodic telemetry system header (includes downlink utility and generated code).
void vff_update_z_conf(float z_meas, float conf)
Definition: vf_float.c:203
struct Vff vff
Definition: vf_float.c:73
void vff_realign(float z_meas)
Definition: vf_float.c:258
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
Vertical filter (in float) estimating altitude, velocity and accel bias.
float bias
accel bias estimate in m/s^2
Definition: vf_float.h:37
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]
covariance matrix
Definition: vf_float.h:40
float z_meas
last measurement
Definition: vf_float.h:39
#define VF_FLOAT_MEAS_NOISE
Definition: vf_float.c:65
#define VFF_STATE_SIZE
static void update_z_conf(float z_meas, float conf)
Definition: vf_float.c:164
void vff_init(float init_z, float init_zdot, float init_bias)
Definition: vf_float.c:89
float z
z-position estimate in m (NED, z-down)
Definition: vf_float.h:35
#define INS_PROPAGATE_FREQUENCY
Definition: vf_float.c:37
#define VF_FLOAT_INIT_PXX
Definition: vf_float.c:55
void vff_update_vz_conf(float vz_meas, float conf)
Definition: vf_float.c:254
#define DT_VFILTER
Definition: vf_float.c:42
void vff_update(float z_meas)
Definition: vf_float.c:199
#define Qzdotzdot
Definition: vf_float.c:70
float zdot
z-velocity estimate in m/s (NED, z-down)
Definition: vf_float.h:36
void vff_init_zero(void)
Definition: vf_float.c:85
#define Qbiasbias
Definition: vf_float.c:71
static void update_vz_conf(float vz, float conf)
Definition: vf_float.c:221