Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
alt_vfilter.c
Go to the documentation of this file.
1 #include "alt_vfilter.h"
2 
3 /*
4 
5 X = [ z zdot bias ]
6 
7 temps :
8  predict 86us
9  update 46us
10 
11 */
12 /* initial covariance diagonal */
13 #define INIT_PXX 10.
14 /* process noise */
15 #define Qzz 0.001
16 #define Qzdotzdot 0.001
17 #define Qbiasbias 0.00001
18 #define R 2.
19 
20 float alt_vf_z;
23 
25 
27 
28 void alt_vf_init(float init_z, float init_zdot, float init_bias) {
29  alt_vf_z = init_z;
30  alt_vf_zdot = init_zdot;
31  alt_vf_bias = init_bias;
32  int i, j;
33  for (i=0; i<STATE_SIZE; i++) {
34  for (j=0; j<STATE_SIZE; j++)
35  alt_vf_P[i][j] = 0.;
36  alt_vf_P[i][i] = INIT_PXX;
37  }
38 
39 }
40 
41 
42 /*
43 
44  F = [ 1 dt -dt^2/2
45  0 1 -dt
46  0 0 1 ];
47 
48  B = [ dt^2/2 dt 0]';
49 
50  Q = [ 0.01 0 0
51  0 0.01 0
52  0 0 0.001 ];
53 
54  Xk1 = F * Xk0 + B * accel;
55 
56  Pk1 = F * Pk0 * F' + Q;
57 
58 */
59 void alt_vf_predict(float accel) {
60  /* update state */
61  float u = accel + 9.81;
62  alt_vf_z = alt_vf_z + DT_VFILTER * alt_vf_zdot;
63  alt_vf_zdot = alt_vf_zdot + DT_VFILTER * ( u - alt_vf_bias);
64  /* update covariance */
65  const float FPF00 = alt_vf_P[0][0] + DT_VFILTER * ( alt_vf_P[1][0] + alt_vf_P[0][1] + DT_VFILTER * alt_vf_P[1][1] );
66  const float FPF01 = alt_vf_P[0][1] + DT_VFILTER * ( alt_vf_P[1][1] - alt_vf_P[0][2] - DT_VFILTER * alt_vf_P[1][2] );
67  const float FPF02 = alt_vf_P[0][2] + DT_VFILTER * ( alt_vf_P[1][2] );
68  const float FPF10 = alt_vf_P[1][0] + DT_VFILTER * (-alt_vf_P[2][0] + alt_vf_P[1][1] - DT_VFILTER * alt_vf_P[2][1] );
69  const float FPF11 = alt_vf_P[1][1] + DT_VFILTER * (-alt_vf_P[2][1] - alt_vf_P[1][2] + DT_VFILTER * alt_vf_P[2][2] );
70  const float FPF12 = alt_vf_P[1][2] + DT_VFILTER * (-alt_vf_P[2][2] );
71  const float FPF20 = alt_vf_P[2][0] + DT_VFILTER * ( alt_vf_P[2][1] );
72  const float FPF21 = alt_vf_P[2][1] + DT_VFILTER * (-alt_vf_P[2][2] );
73  const float FPF22 = alt_vf_P[2][2];
74 
75  alt_vf_P[0][0] = FPF00 + Qzz;
76  alt_vf_P[0][1] = FPF01;
77  alt_vf_P[0][2] = FPF02;
78  alt_vf_P[1][0] = FPF10;
79  alt_vf_P[1][1] = FPF11 + Qzdotzdot;
80  alt_vf_P[1][2] = FPF12;
81  alt_vf_P[2][0] = FPF20;
82  alt_vf_P[2][1] = FPF21;
83  alt_vf_P[2][2] = FPF22 + Qbiasbias;
84 
85 }
86 
87 /*
88  H = [1 0 0];
89  R = 0.1;
90  // state residual
91  y = rangemeter - H * Xm;
92  // covariance residual
93  S = H*Pm*H' + R;
94  // kalman gain
95  K = Pm*H'*inv(S);
96  // update state
97  Xp = Xm + K*y;
98  // update covariance
99  Pp = Pm - K*H*Pm;
100 */
101 void alt_vf_update_z(float z_meas) {
102  alt_vf_z_meas = z_meas;
103 
104  const float y = z_meas - alt_vf_z;
105  const float S = alt_vf_P[0][0] + R;
106  const float K1 = alt_vf_P[0][0] * 1/S;
107  const float K2 = alt_vf_P[1][0] * 1/S;
108  const float K3 = alt_vf_P[2][0] * 1/S;
109 
110  alt_vf_z = alt_vf_z + K1 * y;
111  alt_vf_zdot = alt_vf_zdot + K2 * y;
112  alt_vf_bias = alt_vf_bias + K3 * y;
113 
114  const float P11 = (1. - K1) * alt_vf_P[0][0];
115  const float P12 = (1. - K1) * alt_vf_P[0][1];
116  const float P13 = (1. - K1) * alt_vf_P[0][2];
117  const float P21 = -K2 * alt_vf_P[0][0] + alt_vf_P[1][0];
118  const float P22 = -K2 * alt_vf_P[0][1] + alt_vf_P[1][1];
119  const float P23 = -K2 * alt_vf_P[0][2] + alt_vf_P[1][2];
120  const float P31 = -K3 * alt_vf_P[0][0] + alt_vf_P[2][0];
121  const float P32 = -K3 * alt_vf_P[0][1] + alt_vf_P[2][1];
122  const float P33 = -K3 * alt_vf_P[0][2] + alt_vf_P[2][2];
123 
124  alt_vf_P[0][0] = P11;
125  alt_vf_P[0][1] = P12;
126  alt_vf_P[0][2] = P13;
127  alt_vf_P[1][0] = P21;
128  alt_vf_P[1][1] = P22;
129  alt_vf_P[1][2] = P23;
130  alt_vf_P[2][0] = P31;
131  alt_vf_P[2][1] = P32;
132  alt_vf_P[2][2] = P33;
133 
134 }
135 
136 /*
137  H = [0 1 0];
138  R = 0.1;
139  // state residual
140  yd = vz - H * Xm;
141  // covariance residual
142  S = H*Pm*H' + R;
143  // kalman gain
144  K = Pm*H'*inv(S);
145  // update state
146  Xp = Xm + K*yd;
147  // update covariance
148  Pp = Pm - K*H*Pm;
149 */
150 void alt_vf_update_vz(float vz) {
151  const float yd = vz - alt_vf_zdot;
152  const float S = alt_vf_P[1][1] + R;
153  const float K1 = alt_vf_P[0][1] * 1/S;
154  const float K2 = alt_vf_P[1][1] * 1/S;
155  const float K3 = alt_vf_P[2][1] * 1/S;
156 
157  alt_vf_z = alt_vf_z + K1 * yd;
158  alt_vf_zdot = alt_vf_zdot + K2 * yd;
159  alt_vf_bias = alt_vf_bias + K3 * yd;
160 
161  const float P11 = -K1 * alt_vf_P[1][0] + alt_vf_P[0][0];
162  const float P12 = -K1 * alt_vf_P[1][1] + alt_vf_P[0][1];
163  const float P13 = -K1 * alt_vf_P[1][2] + alt_vf_P[0][2];
164  const float P21 = (1. - K2) * alt_vf_P[1][0];
165  const float P22 = (1. - K2) * alt_vf_P[1][1];
166  const float P23 = (1. - K2) * alt_vf_P[1][2];
167  const float P31 = -K3 * alt_vf_P[1][0] + alt_vf_P[2][0];
168  const float P32 = -K3 * alt_vf_P[1][1] + alt_vf_P[2][1];
169  const float P33 = -K3 * alt_vf_P[1][2] + alt_vf_P[2][2];
170 
171  alt_vf_P[0][0] = P11;
172  alt_vf_P[0][1] = P12;
173  alt_vf_P[0][2] = P13;
174  alt_vf_P[1][0] = P21;
175  alt_vf_P[1][1] = P22;
176  alt_vf_P[1][2] = P23;
177  alt_vf_P[2][0] = P31;
178  alt_vf_P[2][1] = P32;
179  alt_vf_P[2][2] = P33;
180 
181 }
182 
183 
#define INIT_PXX
Definition: alt_vfilter.c:13
float alt_vf_z_meas
Definition: alt_vfilter.c:26
float alt_vf_z
Definition: alt_vfilter.c:20
void alt_vf_update_vz(float vz)
Definition: alt_vfilter.c:150
void alt_vf_update_z(float z_meas)
Definition: alt_vfilter.c:101
void alt_vf_predict(float accel)
Definition: alt_vfilter.c:59
float alt_vf_bias
Definition: alt_vfilter.c:22
int32_t int32_t accel
#define STATE_SIZE
Definition: agl_vfilter.h:6
#define Qzz
Definition: alt_vfilter.c:15
float alt_vf_zdot
Definition: alt_vfilter.c:21
float alt_vf_P[STATE_SIZE][STATE_SIZE]
Definition: alt_vfilter.c:24
void alt_vf_init(float init_z, float init_zdot, float init_bias)
Definition: alt_vfilter.c:28
#define Qbiasbias
Definition: alt_vfilter.c:17
#define Qzdotzdot
Definition: alt_vfilter.c:16
#define R
Definition: alt_vfilter.c:18