Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
vf_float.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
25 
26 /*
27 
28 X = [ z zdot bias ]
29 
30 temps :
31  propagate 86us
32  update 46us
33 
34 */
35 /* initial covariance diagonal */
36 #define INIT_PXX 1.
37 /* process noise */
38 #define ACCEL_NOISE 0.5
39 #define Qzz ACCEL_NOISE/512./512./2.
40 #define Qzdotzdot ACCEL_NOISE/512.
41 #define Qbiasbias 1e-7
42 #define R 1.
43 
44 float vff_z;
45 float vff_bias;
46 float vff_zdot;
48 
50 
51 float vff_z_meas;
52 
53 void vff_init(float init_z, float init_zdot, float init_bias) {
54  vff_z = init_z;
55  vff_zdot = init_zdot;
56  vff_bias = init_bias;
57  int i, j;
58  for (i=0; i<VFF_STATE_SIZE; i++) {
59  for (j=0; j<VFF_STATE_SIZE; j++)
60  vff_P[i][j] = 0.;
61  vff_P[i][i] = INIT_PXX;
62  }
63 
64 }
65 
66 
67 /*
68 
69  F = [ 1 dt -dt^2/2
70  0 1 -dt
71  0 0 1 ];
72 
73  B = [ dt^2/2 dt 0]';
74 
75  Q = [ 0.01 0 0
76  0 0.01 0
77  0 0 0.001 ];
78 
79  Xk1 = F * Xk0 + B * accel;
80 
81  Pk1 = F * Pk0 * F' + Q;
82 
83 */
84 void vff_propagate(float accel) {
85  /* update state */
86  vff_zdotdot = accel + 9.81 - vff_bias;
87  vff_z = vff_z + DT_VFILTER * vff_zdot;
88  vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
89  /* update covariance */
90  const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
91  const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
92  const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
93  const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
94  const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
95  const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
96  const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
97  const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
98  const float FPF22 = vff_P[2][2];
99 
100  vff_P[0][0] = FPF00 + Qzz;
101  vff_P[0][1] = FPF01;
102  vff_P[0][2] = FPF02;
103  vff_P[1][0] = FPF10;
104  vff_P[1][1] = FPF11 + Qzdotzdot;
105  vff_P[1][2] = FPF12;
106  vff_P[2][0] = FPF20;
107  vff_P[2][1] = FPF21;
108  vff_P[2][2] = FPF22 + Qbiasbias;
109 
110 }
111 /*
112  H = [1 0 0];
113  R = 0.1;
114  // state residual
115  y = rangemeter - H * Xm;
116  // covariance residual
117  S = H*Pm*H' + R;
118  // kalman gain
119  K = Pm*H'*inv(S);
120  // update state
121  Xp = Xm + K*y;
122  // update covariance
123  Pp = Pm - K*H*Pm;
124 */
125 __attribute__ ((always_inline)) static inline void update_z_conf(float z_meas, float conf) {
126  vff_z_meas = z_meas;
127 
128  const float y = z_meas - vff_z;
129  const float S = vff_P[0][0] + conf;
130  const float K1 = vff_P[0][0] * 1/S;
131  const float K2 = vff_P[1][0] * 1/S;
132  const float K3 = vff_P[2][0] * 1/S;
133 
134  vff_z = vff_z + K1 * y;
135  vff_zdot = vff_zdot + K2 * y;
136  vff_bias = vff_bias + K3 * y;
137 
138  const float P11 = (1. - K1) * vff_P[0][0];
139  const float P12 = (1. - K1) * vff_P[0][1];
140  const float P13 = (1. - K1) * vff_P[0][2];
141  const float P21 = -K2 * vff_P[0][0] + vff_P[1][0];
142  const float P22 = -K2 * vff_P[0][1] + vff_P[1][1];
143  const float P23 = -K2 * vff_P[0][2] + vff_P[1][2];
144  const float P31 = -K3 * vff_P[0][0] + vff_P[2][0];
145  const float P32 = -K3 * vff_P[0][1] + vff_P[2][1];
146  const float P33 = -K3 * vff_P[0][2] + vff_P[2][2];
147 
148  vff_P[0][0] = P11;
149  vff_P[0][1] = P12;
150  vff_P[0][2] = P13;
151  vff_P[1][0] = P21;
152  vff_P[1][1] = P22;
153  vff_P[1][2] = P23;
154  vff_P[2][0] = P31;
155  vff_P[2][1] = P32;
156  vff_P[2][2] = P33;
157 
158 }
159 
160 void vff_update(float z_meas) {
161  update_z_conf(z_meas, R);
162 }
163 
164 void vff_update_z_conf(float z_meas, float conf) {
165  update_z_conf(z_meas, conf);
166 }
167 
168 /*
169  H = [0 1 0];
170  R = 0.1;
171  // state residual
172  yd = vz - H * Xm;
173  // covariance residual
174  S = H*Pm*H' + R;
175  // kalman gain
176  K = Pm*H'*inv(S);
177  // update state
178  Xp = Xm + K*yd;
179  // update covariance
180  Pp = Pm - K*H*Pm;
181 */
182 __attribute__ ((always_inline)) static inline void update_vz_conf(float vz, float conf) {
183  const float yd = vz - vff_zdot;
184  const float S = vff_P[1][1] + conf;
185  const float K1 = vff_P[0][1] * 1/S;
186  const float K2 = vff_P[1][1] * 1/S;
187  const float K3 = vff_P[2][1] * 1/S;
188 
189  vff_z = vff_z + K1 * yd;
190  vff_zdot = vff_zdot + K2 * yd;
191  vff_bias = vff_bias + K3 * yd;
192 
193  const float P11 = -K1 * vff_P[1][0] + vff_P[0][0];
194  const float P12 = -K1 * vff_P[1][1] + vff_P[0][1];
195  const float P13 = -K1 * vff_P[1][2] + vff_P[0][2];
196  const float P21 = (1. - K2) * vff_P[1][0];
197  const float P22 = (1. - K2) * vff_P[1][1];
198  const float P23 = (1. - K2) * vff_P[1][2];
199  const float P31 = -K3 * vff_P[1][0] + vff_P[2][0];
200  const float P32 = -K3 * vff_P[1][1] + vff_P[2][1];
201  const float P33 = -K3 * vff_P[1][2] + vff_P[2][2];
202 
203  vff_P[0][0] = P11;
204  vff_P[0][1] = P12;
205  vff_P[0][2] = P13;
206  vff_P[1][0] = P21;
207  vff_P[1][1] = P22;
208  vff_P[1][2] = P23;
209  vff_P[2][0] = P31;
210  vff_P[2][1] = P32;
211  vff_P[2][2] = P33;
212 
213 }
214 
215 void vff_update_vz_conf(float vz_meas, float conf) {
216  update_vz_conf(vz_meas, conf);
217 }
218 
219 void vff_realign(float z_meas) {
220  vff_z = z_meas;
221  vff_zdot = 0;
222 }
#define VFF_STATE_SIZE
Definition: vf_float.h:27
void vff_propagate(float accel)
Definition: vf_float.c:84
float vff_z_meas
Definition: vf_float.c:51
#define Qzz
Definition: vf_float.c:39
__attribute__((always_inline))
Definition: vf_float.c:125
void vff_update_z_conf(float z_meas, float conf)
Definition: vf_float.c:164
void vff_realign(float z_meas)
Definition: vf_float.c:219
float vff_zdot
Definition: vf_float.c:46
float vff_bias
Definition: vf_float.c:45
#define INIT_PXX
Definition: vf_float.c:36
void vff_init(float init_z, float init_zdot, float init_bias)
Definition: vf_float.c:53
int32_t int32_t accel
float vff_zdotdot
Definition: vf_float.c:47
float y
Definition: hf_float.h:57
void vff_update_vz_conf(float vz_meas, float conf)
Definition: vf_float.c:215
void vff_update(float z_meas)
Definition: vf_float.c:160
#define Qzdotzdot
Definition: vf_float.c:40
#define Qbiasbias
Definition: vf_float.c:41
float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]
Definition: vf_float.c:49
#define R
Definition: vf_float.c:42
float vff_z
Definition: vf_float.c:44