Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 
31 /*
32 
33 X = [ z zdot bias ]
34 
35 temps :
36  propagate 86us
37  update 46us
38 
39 */
40 /* initial error covariance diagonal */
41 #ifndef VF_FLOAT_INIT_PXX
42 #define VF_FLOAT_INIT_PXX 1.0
43 #endif
44 
45 /* process noise covariance Q */
46 #ifndef VF_FLOAT_ACCEL_NOISE
47 #define VF_FLOAT_ACCEL_NOISE 0.5
48 #endif
49 
50 /* measurement noise covariance R */
51 #ifndef VF_FLOAT_MEAS_NOISE
52 #define VF_FLOAT_MEAS_NOISE 1.0
53 #endif
54 
55 /* default parameters */
56 #define Qzz VF_FLOAT_ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
57 #define Qzdotzdot VF_FLOAT_ACCEL_NOISE * DT_VFILTER
58 #define Qbiasbias 1e-7
59 
60 float vff_z;
61 float vff_bias;
62 float vff_zdot;
64 
66 
67 float vff_z_meas;
68 
69 void vff_init(float init_z, float init_zdot, float init_bias) {
70  vff_z = init_z;
71  vff_zdot = init_zdot;
72  vff_bias = init_bias;
73  int i, j;
74  for (i=0; i<VFF_STATE_SIZE; i++) {
75  for (j=0; j<VFF_STATE_SIZE; j++)
76  vff_P[i][j] = 0.;
77  vff_P[i][i] = VF_FLOAT_INIT_PXX;
78  }
79 
80 }
81 
82 
83 /*
84 
85  F = [ 1 dt -dt^2/2
86  0 1 -dt
87  0 0 1 ];
88 
89  B = [ dt^2/2 dt 0]';
90 
91  Q = [ 0.01 0 0
92  0 0.01 0
93  0 0 0.001 ];
94 
95  Xk1 = F * Xk0 + B * accel;
96 
97  Pk1 = F * Pk0 * F' + Q;
98 
99 */
100 void vff_propagate(float accel) {
101  /* update state (Xk1) */
102  vff_zdotdot = accel + 9.81 - vff_bias;
103  vff_z = vff_z + DT_VFILTER * vff_zdot;
104  vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
105  /* update covariance (Pk1) */
106  const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
107  const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
108  const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
109  const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
110  const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
111  const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
112  const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
113  const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
114  const float FPF22 = vff_P[2][2];
115 
116  vff_P[0][0] = FPF00 + Qzz;
117  vff_P[0][1] = FPF01;
118  vff_P[0][2] = FPF02;
119  vff_P[1][0] = FPF10;
120  vff_P[1][1] = FPF11 + Qzdotzdot;
121  vff_P[1][2] = FPF12;
122  vff_P[2][0] = FPF20;
123  vff_P[2][1] = FPF21;
124  vff_P[2][2] = FPF22 + Qbiasbias;
125 
126 }
127 /*
128  H = [1 0 0];
129  R = 0.1;
130  // state residual
131  y = rangemeter - H * Xm;
132  // covariance residual
133  S = H*Pm*H' + R;
134  // kalman gain
135  K = Pm*H'*inv(S);
136  // update state
137  Xp = Xm + K*y;
138  // update covariance
139  Pp = Pm - K*H*Pm;
140 */
141 static inline void update_z_conf(float z_meas, float conf) {
142  vff_z_meas = z_meas;
143 
144  const float y = z_meas - vff_z;
145  const float S = vff_P[0][0] + conf;
146  const float K1 = vff_P[0][0] * 1/S;
147  const float K2 = vff_P[1][0] * 1/S;
148  const float K3 = vff_P[2][0] * 1/S;
149 
150  vff_z = vff_z + K1 * y;
151  vff_zdot = vff_zdot + K2 * y;
152  vff_bias = vff_bias + K3 * y;
153 
154  const float P11 = (1. - K1) * vff_P[0][0];
155  const float P12 = (1. - K1) * vff_P[0][1];
156  const float P13 = (1. - K1) * vff_P[0][2];
157  const float P21 = -K2 * vff_P[0][0] + vff_P[1][0];
158  const float P22 = -K2 * vff_P[0][1] + vff_P[1][1];
159  const float P23 = -K2 * vff_P[0][2] + vff_P[1][2];
160  const float P31 = -K3 * vff_P[0][0] + vff_P[2][0];
161  const float P32 = -K3 * vff_P[0][1] + vff_P[2][1];
162  const float P33 = -K3 * vff_P[0][2] + vff_P[2][2];
163 
164  vff_P[0][0] = P11;
165  vff_P[0][1] = P12;
166  vff_P[0][2] = P13;
167  vff_P[1][0] = P21;
168  vff_P[1][1] = P22;
169  vff_P[1][2] = P23;
170  vff_P[2][0] = P31;
171  vff_P[2][1] = P32;
172  vff_P[2][2] = P33;
173 
174 }
175 
176 void vff_update(float z_meas) {
178 }
179 
180 void vff_update_z_conf(float z_meas, float conf) {
181  update_z_conf(z_meas, conf);
182 }
183 
184 /*
185  H = [0 1 0];
186  R = 0.1;
187  // state residual
188  yd = vz - H * Xm;
189  // covariance residual
190  S = H*Pm*H' + R;
191  // kalman gain
192  K = Pm*H'*inv(S);
193  // update state
194  Xp = Xm + K*yd;
195  // update covariance
196  Pp = Pm - K*H*Pm;
197 */
198 static inline void update_vz_conf(float vz, float conf) {
199  const float yd = vz - vff_zdot;
200  const float S = vff_P[1][1] + conf;
201  const float K1 = vff_P[0][1] * 1/S;
202  const float K2 = vff_P[1][1] * 1/S;
203  const float K3 = vff_P[2][1] * 1/S;
204 
205  vff_z = vff_z + K1 * yd;
206  vff_zdot = vff_zdot + K2 * yd;
207  vff_bias = vff_bias + K3 * yd;
208 
209  const float P11 = -K1 * vff_P[1][0] + vff_P[0][0];
210  const float P12 = -K1 * vff_P[1][1] + vff_P[0][1];
211  const float P13 = -K1 * vff_P[1][2] + vff_P[0][2];
212  const float P21 = (1. - K2) * vff_P[1][0];
213  const float P22 = (1. - K2) * vff_P[1][1];
214  const float P23 = (1. - K2) * vff_P[1][2];
215  const float P31 = -K3 * vff_P[1][0] + vff_P[2][0];
216  const float P32 = -K3 * vff_P[1][1] + vff_P[2][1];
217  const float P33 = -K3 * vff_P[1][2] + vff_P[2][2];
218 
219  vff_P[0][0] = P11;
220  vff_P[0][1] = P12;
221  vff_P[0][2] = P13;
222  vff_P[1][0] = P21;
223  vff_P[1][1] = P22;
224  vff_P[1][2] = P23;
225  vff_P[2][0] = P31;
226  vff_P[2][1] = P32;
227  vff_P[2][2] = P33;
228 
229 }
230 
231 void vff_update_vz_conf(float vz_meas, float conf) {
232  update_vz_conf(vz_meas, conf);
233 }
234 
235 void vff_realign(float z_meas) {
236  vff_z = z_meas;
237  vff_zdot = 0;
238 }
void vff_propagate(float accel)
Definition: vf_float.c:100
float vff_z_meas
Definition: vf_float.c:67
#define Qzz
Definition: vf_float.c:56
void vff_update_z_conf(float z_meas, float conf)
Definition: vf_float.c:180
void vff_realign(float z_meas)
Definition: vf_float.c:235
Vertical filter (in float) estimating altitude, velocity and accel bias.
float vff_zdot
Definition: vf_float.c:62
#define VF_FLOAT_MEAS_NOISE
Definition: vf_float.c:52
#define VFF_STATE_SIZE
float vff_bias
Definition: vf_float.c:61
static void update_z_conf(float z_meas, float conf)
Definition: vf_float.c:141
void vff_init(float init_z, float init_zdot, float init_bias)
Definition: vf_float.c:69
float vff_zdotdot
Definition: vf_float.c:63
#define VF_FLOAT_INIT_PXX
Definition: vf_float.c:42
void vff_update_vz_conf(float vz_meas, float conf)
Definition: vf_float.c:231
void vff_update(float z_meas)
Definition: vf_float.c:176
#define Qzdotzdot
Definition: vf_float.c:57
#define Qbiasbias
Definition: vf_float.c:58
float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]
Definition: vf_float.c:65
static void update_vz_conf(float vz, float conf)
Definition: vf_float.c:198
float vff_z
Definition: vf_float.c:60