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
agl_vfilter.c
Go to the documentation of this file.
1 #include "agl_vfilter.h"
2 
3 #include BOARD_CONFIG
4 #include "generated/airframe.h"
5 #include "std.h"
6 
7 /*
8 
9 X = [ z zdot bias ]
10 
11 temps :
12  predict 86us
13  update 46us
14 
15 */
16 /* initial covariance diagonal */
17 #define INIT_PXX 10.
18 /* process noise */
19 #define Qzz 0.001
20 #define Qzdotzdot 0.001
21 #define Qbiasbias 0.00001
22 #define R 2.
23 
24 float agl_vf_z;
27 
29 
31 
33 
34 void agl_vf_init(float init_z, float init_zdot, float init_bias) {
35  agl_vf_z = init_z;
36  agl_vf_zdot = init_zdot;
37  agl_vf_bias = init_bias;
38  int i, j;
39  for (i=0; i<STATE_SIZE; i++) {
40  for (j=0; j<STATE_SIZE; j++)
41  agl_vf_P[i][j] = 0.;
42  agl_vf_P[i][i] = INIT_PXX;
43  }
44  adc_buf_channel(ADC_CHANNEL_TELEMETER, &agl_adc_buf, DEFAULT_AV_NB_SAMPLE);
45 
46 }
47 
48 
49 /*
50 
51  F = [ 1 dt -dt^2/2
52  0 1 -dt
53  0 0 1 ];
54 
55  B = [ dt^2/2 dt 0]';
56 
57  Q = [ 0.01 0 0
58  0 0.01 0
59  0 0 0.001 ];
60 
61  Xk1 = F * Xk0 + B * accel;
62 
63  Pk1 = F * Pk0 * F' + Q;
64 
65 */
66 void agl_vf_predict(float accel) {
67  /* update state */
68  float u = accel + 9.81;
69  agl_vf_z = agl_vf_z + DT_VFILTER * agl_vf_zdot;
70  agl_vf_zdot = agl_vf_zdot + DT_VFILTER * ( u - agl_vf_bias);
71  /* update covariance */
72  const float FPF00 = agl_vf_P[0][0] + DT_VFILTER * ( agl_vf_P[1][0] + agl_vf_P[0][1] + DT_VFILTER * agl_vf_P[1][1] );
73  const float FPF01 = agl_vf_P[0][1] + DT_VFILTER * ( agl_vf_P[1][1] - agl_vf_P[0][2] - DT_VFILTER * agl_vf_P[1][2] );
74  const float FPF02 = agl_vf_P[0][2] + DT_VFILTER * ( agl_vf_P[1][2] );
75  const float FPF10 = agl_vf_P[1][0] + DT_VFILTER * (-agl_vf_P[2][0] + agl_vf_P[1][1] - DT_VFILTER * agl_vf_P[2][1] );
76  const float FPF11 = agl_vf_P[1][1] + DT_VFILTER * (-agl_vf_P[2][1] - agl_vf_P[1][2] + DT_VFILTER * agl_vf_P[2][2] );
77  const float FPF12 = agl_vf_P[1][2] + DT_VFILTER * (-agl_vf_P[2][2] );
78  const float FPF20 = agl_vf_P[2][0] + DT_VFILTER * ( agl_vf_P[2][1] );
79  const float FPF21 = agl_vf_P[2][1] + DT_VFILTER * (-agl_vf_P[2][2] );
80  const float FPF22 = agl_vf_P[2][2];
81 
82  agl_vf_P[0][0] = FPF00 + Qzz;
83  agl_vf_P[0][1] = FPF01;
84  agl_vf_P[0][2] = FPF02;
85  agl_vf_P[1][0] = FPF10;
86  agl_vf_P[1][1] = FPF11 + Qzdotzdot;
87  agl_vf_P[1][2] = FPF12;
88  agl_vf_P[2][0] = FPF20;
89  agl_vf_P[2][1] = FPF21;
90  agl_vf_P[2][2] = FPF22 + Qbiasbias;
91 
92 }
93 /*
94  H = [1 0 0];
95  R = 0.1;
96  // state residual
97  y = rangemeter - H * Xm;
98  // covariance residual
99  S = H*Pm*H' + R;
100  // kalman gain
101  K = Pm*H'*inv(S);
102  // update state
103  Xp = Xm + K*y;
104  // update covariance
105  Pp = Pm - K*H*Pm;
106 */
107 void agl_vf_update(float z_meas) {
108  agl_vf_z_meas = z_meas;
109 
110  const float y = z_meas - agl_vf_z;
111  const float S = agl_vf_P[0][0] + R;
112  const float K1 = agl_vf_P[0][0] * 1/S;
113  const float K2 = agl_vf_P[1][0] * 1/S;
114  const float K3 = agl_vf_P[2][0] * 1/S;
115 
116  agl_vf_z = agl_vf_z + K1 * y;
117  agl_vf_zdot = agl_vf_zdot + K2 * y;
118  agl_vf_bias = agl_vf_bias + K3 * y;
119 
120  const float P11 = (1. - K1) * agl_vf_P[0][0];
121  const float P12 = (1. - K1) * agl_vf_P[0][1];
122  const float P13 = (1. - K1) * agl_vf_P[0][2];
123  const float P21 = -K2 * agl_vf_P[0][0] + agl_vf_P[1][0];
124  const float P22 = -K2 * agl_vf_P[0][1] + agl_vf_P[1][1];
125  const float P23 = -K2 * agl_vf_P[0][2] + agl_vf_P[1][2];
126  const float P31 = -K3 * agl_vf_P[0][0] + agl_vf_P[2][0];
127  const float P32 = -K3 * agl_vf_P[0][1] + agl_vf_P[2][1];
128  const float P33 = -K3 * agl_vf_P[0][2] + agl_vf_P[2][2];
129 
130  agl_vf_P[0][0] = P11;
131  agl_vf_P[0][1] = P12;
132  agl_vf_P[0][2] = P13;
133  agl_vf_P[1][0] = P21;
134  agl_vf_P[1][1] = P22;
135  agl_vf_P[1][2] = P23;
136  agl_vf_P[2][0] = P31;
137  agl_vf_P[2][1] = P32;
138  agl_vf_P[2][2] = P33;
139 
140 }
141 
void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample)
Registers a buffer to be used to store the specified converted channel Usage:
Definition: adc_arch.c:59
float agl_vf_zdot
Definition: agl_vfilter.c:25
#define Qzz
Definition: agl_vfilter.c:19
void agl_vf_predict(float accel)
Definition: agl_vfilter.c:66
float agl_vf_P[STATE_SIZE][STATE_SIZE]
Definition: agl_vfilter.c:30
float agl_vf_z
Definition: agl_vfilter.c:24
Generic interface for all ADC hardware drivers, independent from microcontroller architecture.
Definition: adc.h:60
void agl_vf_update(float z_meas)
Definition: agl_vfilter.c:107
void agl_vf_init(float init_z, float init_zdot, float init_bias)
Definition: agl_vfilter.c:34
#define Qbiasbias
Definition: agl_vfilter.c:21
#define Qzdotzdot
Definition: agl_vfilter.c:20
#define STATE_SIZE
Definition: agl_vfilter.h:6
#define R
Definition: agl_vfilter.c:22
struct adc_buf agl_adc_buf
Definition: agl_vfilter.c:28
#define INIT_PXX
Definition: agl_vfilter.c:17
#define DEFAULT_AV_NB_SAMPLE
Definition: adc.h:48
float agl_vf_z_meas
Definition: agl_vfilter.c:32
float agl_vf_bias
Definition: agl_vfilter.c:26