mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 07:53:43 +08:00
kalman filter for barometer and telemeter (coming from coaxial autopilot)
This commit is contained in:
Gautier Hattenberger
@@ -0,0 +1,141 @@
|
||||
#include "agl_vfilter.h"
|
||||
|
||||
#include CONFIG
|
||||
#include "airframe.h"
|
||||
#include "std.h"
|
||||
|
||||
/*
|
||||
|
||||
X = [ z zdot bias ]
|
||||
|
||||
temps :
|
||||
predict 86us
|
||||
update 46us
|
||||
|
||||
*/
|
||||
/* initial covariance diagonal */
|
||||
#define INIT_PXX 10.
|
||||
/* process noise */
|
||||
#define Qzz 0.001
|
||||
#define Qzdotzdot 0.001
|
||||
#define Qbiasbias 0.00001
|
||||
#define R 2.
|
||||
|
||||
float agl_vf_z;
|
||||
float agl_vf_zdot;
|
||||
float agl_vf_bias;
|
||||
|
||||
struct adc_buf agl_adc_buf;
|
||||
|
||||
float agl_vf_P[STATE_SIZE][STATE_SIZE];
|
||||
|
||||
float agl_vf_z_meas;
|
||||
|
||||
void agl_vf_init(float init_z, float init_zdot, float init_bias) {
|
||||
agl_vf_z = init_z;
|
||||
agl_vf_zdot = init_zdot;
|
||||
agl_vf_bias = init_bias;
|
||||
int i, j;
|
||||
for (i=0; i<STATE_SIZE; i++) {
|
||||
for (j=0; j<STATE_SIZE; j++)
|
||||
agl_vf_P[i][j] = 0.;
|
||||
agl_vf_P[i][i] = INIT_PXX;
|
||||
}
|
||||
adc_buf_channel(ADC_CHANNEL_TELEMETER, &agl_adc_buf, DEFAULT_AV_NB_SAMPLE);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
F = [ 1 dt -dt^2/2
|
||||
0 1 -dt
|
||||
0 0 1 ];
|
||||
|
||||
B = [ dt^2/2 dt 0]';
|
||||
|
||||
Q = [ 0.01 0 0
|
||||
0 0.01 0
|
||||
0 0 0.001 ];
|
||||
|
||||
Xk1 = F * Xk0 + B * accel;
|
||||
|
||||
Pk1 = F * Pk0 * F' + Q;
|
||||
|
||||
*/
|
||||
void agl_vf_predict(float accel) {
|
||||
/* update state */
|
||||
float u = accel + 9.81;
|
||||
agl_vf_z = agl_vf_z + DT_VFILTER * agl_vf_zdot;
|
||||
agl_vf_zdot = agl_vf_zdot + DT_VFILTER * ( u - agl_vf_bias);
|
||||
/* update covariance */
|
||||
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] );
|
||||
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] );
|
||||
const float FPF02 = agl_vf_P[0][2] + DT_VFILTER * ( agl_vf_P[1][2] );
|
||||
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] );
|
||||
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] );
|
||||
const float FPF12 = agl_vf_P[1][2] + DT_VFILTER * (-agl_vf_P[2][2] );
|
||||
const float FPF20 = agl_vf_P[2][0] + DT_VFILTER * ( agl_vf_P[2][1] );
|
||||
const float FPF21 = agl_vf_P[2][1] + DT_VFILTER * (-agl_vf_P[2][2] );
|
||||
const float FPF22 = agl_vf_P[2][2];
|
||||
|
||||
agl_vf_P[0][0] = FPF00 + Qzz;
|
||||
agl_vf_P[0][1] = FPF01;
|
||||
agl_vf_P[0][2] = FPF02;
|
||||
agl_vf_P[1][0] = FPF10;
|
||||
agl_vf_P[1][1] = FPF11 + Qzdotzdot;
|
||||
agl_vf_P[1][2] = FPF12;
|
||||
agl_vf_P[2][0] = FPF20;
|
||||
agl_vf_P[2][1] = FPF21;
|
||||
agl_vf_P[2][2] = FPF22 + Qbiasbias;
|
||||
|
||||
}
|
||||
/*
|
||||
H = [1 0 0];
|
||||
R = 0.1;
|
||||
// state residual
|
||||
y = rangemeter - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*y;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
void agl_vf_update(float z_meas) {
|
||||
agl_vf_z_meas = z_meas;
|
||||
|
||||
const float y = z_meas - agl_vf_z;
|
||||
const float S = agl_vf_P[0][0] + R;
|
||||
const float K1 = agl_vf_P[0][0] * 1/S;
|
||||
const float K2 = agl_vf_P[1][0] * 1/S;
|
||||
const float K3 = agl_vf_P[2][0] * 1/S;
|
||||
|
||||
agl_vf_z = agl_vf_z + K1 * y;
|
||||
agl_vf_zdot = agl_vf_zdot + K2 * y;
|
||||
agl_vf_bias = agl_vf_bias + K3 * y;
|
||||
|
||||
const float P11 = (1. - K1) * agl_vf_P[0][0];
|
||||
const float P12 = (1. - K1) * agl_vf_P[0][1];
|
||||
const float P13 = (1. - K1) * agl_vf_P[0][2];
|
||||
const float P21 = -K2 * agl_vf_P[0][0] + agl_vf_P[1][0];
|
||||
const float P22 = -K2 * agl_vf_P[0][1] + agl_vf_P[1][1];
|
||||
const float P23 = -K2 * agl_vf_P[0][2] + agl_vf_P[1][2];
|
||||
const float P31 = -K3 * agl_vf_P[0][0] + agl_vf_P[2][0];
|
||||
const float P32 = -K3 * agl_vf_P[0][1] + agl_vf_P[2][1];
|
||||
const float P33 = -K3 * agl_vf_P[0][2] + agl_vf_P[2][2];
|
||||
|
||||
agl_vf_P[0][0] = P11;
|
||||
agl_vf_P[0][1] = P12;
|
||||
agl_vf_P[0][2] = P13;
|
||||
agl_vf_P[1][0] = P21;
|
||||
agl_vf_P[1][1] = P22;
|
||||
agl_vf_P[1][2] = P23;
|
||||
agl_vf_P[2][0] = P31;
|
||||
agl_vf_P[2][1] = P32;
|
||||
agl_vf_P[2][2] = P33;
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
#ifndef AGL_VFILTER_H
|
||||
#define AGL_VFILTER_H
|
||||
|
||||
#include "adc.h"
|
||||
|
||||
#define STATE_SIZE 3
|
||||
|
||||
extern float agl_vf_z;
|
||||
extern float agl_vf_zdot;
|
||||
extern float agl_vf_bias;
|
||||
extern float agl_vf_P[STATE_SIZE][STATE_SIZE];
|
||||
|
||||
extern struct adc_buf agl_adc_buf;
|
||||
|
||||
extern float agl_vf_z_meas;
|
||||
|
||||
extern void agl_vf_init(float z, float zdot, float bias);
|
||||
extern void agl_vf_predict(float accel);
|
||||
extern void agl_vf_update(float z_meas);
|
||||
|
||||
#endif /* AGL_VFILTER_H */
|
||||
@@ -0,0 +1,183 @@
|
||||
#include "alt_vfilter.h"
|
||||
|
||||
/*
|
||||
|
||||
X = [ z zdot bias ]
|
||||
|
||||
temps :
|
||||
predict 86us
|
||||
update 46us
|
||||
|
||||
*/
|
||||
/* initial covariance diagonal */
|
||||
#define INIT_PXX 10.
|
||||
/* process noise */
|
||||
#define Qzz 0.001
|
||||
#define Qzdotzdot 0.001
|
||||
#define Qbiasbias 0.00001
|
||||
#define R 2.
|
||||
|
||||
float alt_vf_z;
|
||||
float alt_vf_zdot;
|
||||
float alt_vf_bias;
|
||||
|
||||
float alt_vf_P[STATE_SIZE][STATE_SIZE];
|
||||
|
||||
float alt_vf_z_meas;
|
||||
|
||||
void alt_vf_init(float init_z, float init_zdot, float init_bias) {
|
||||
alt_vf_z = init_z;
|
||||
alt_vf_zdot = init_zdot;
|
||||
alt_vf_bias = init_bias;
|
||||
int i, j;
|
||||
for (i=0; i<STATE_SIZE; i++) {
|
||||
for (j=0; j<STATE_SIZE; j++)
|
||||
alt_vf_P[i][j] = 0.;
|
||||
alt_vf_P[i][i] = INIT_PXX;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
F = [ 1 dt -dt^2/2
|
||||
0 1 -dt
|
||||
0 0 1 ];
|
||||
|
||||
B = [ dt^2/2 dt 0]';
|
||||
|
||||
Q = [ 0.01 0 0
|
||||
0 0.01 0
|
||||
0 0 0.001 ];
|
||||
|
||||
Xk1 = F * Xk0 + B * accel;
|
||||
|
||||
Pk1 = F * Pk0 * F' + Q;
|
||||
|
||||
*/
|
||||
void alt_vf_predict(float accel) {
|
||||
/* update state */
|
||||
float u = accel + 9.81;
|
||||
alt_vf_z = alt_vf_z + DT_VFILTER * alt_vf_zdot;
|
||||
alt_vf_zdot = alt_vf_zdot + DT_VFILTER * ( u - alt_vf_bias);
|
||||
/* update covariance */
|
||||
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] );
|
||||
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] );
|
||||
const float FPF02 = alt_vf_P[0][2] + DT_VFILTER * ( alt_vf_P[1][2] );
|
||||
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] );
|
||||
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] );
|
||||
const float FPF12 = alt_vf_P[1][2] + DT_VFILTER * (-alt_vf_P[2][2] );
|
||||
const float FPF20 = alt_vf_P[2][0] + DT_VFILTER * ( alt_vf_P[2][1] );
|
||||
const float FPF21 = alt_vf_P[2][1] + DT_VFILTER * (-alt_vf_P[2][2] );
|
||||
const float FPF22 = alt_vf_P[2][2];
|
||||
|
||||
alt_vf_P[0][0] = FPF00 + Qzz;
|
||||
alt_vf_P[0][1] = FPF01;
|
||||
alt_vf_P[0][2] = FPF02;
|
||||
alt_vf_P[1][0] = FPF10;
|
||||
alt_vf_P[1][1] = FPF11 + Qzdotzdot;
|
||||
alt_vf_P[1][2] = FPF12;
|
||||
alt_vf_P[2][0] = FPF20;
|
||||
alt_vf_P[2][1] = FPF21;
|
||||
alt_vf_P[2][2] = FPF22 + Qbiasbias;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
H = [1 0 0];
|
||||
R = 0.1;
|
||||
// state residual
|
||||
y = rangemeter - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*y;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
void alt_vf_update_z(float z_meas) {
|
||||
alt_vf_z_meas = z_meas;
|
||||
|
||||
const float y = z_meas - alt_vf_z;
|
||||
const float S = alt_vf_P[0][0] + R;
|
||||
const float K1 = alt_vf_P[0][0] * 1/S;
|
||||
const float K2 = alt_vf_P[1][0] * 1/S;
|
||||
const float K3 = alt_vf_P[2][0] * 1/S;
|
||||
|
||||
alt_vf_z = alt_vf_z + K1 * y;
|
||||
alt_vf_zdot = alt_vf_zdot + K2 * y;
|
||||
alt_vf_bias = alt_vf_bias + K3 * y;
|
||||
|
||||
const float P11 = (1. - K1) * alt_vf_P[0][0];
|
||||
const float P12 = (1. - K1) * alt_vf_P[0][1];
|
||||
const float P13 = (1. - K1) * alt_vf_P[0][2];
|
||||
const float P21 = -K2 * alt_vf_P[0][0] + alt_vf_P[1][0];
|
||||
const float P22 = -K2 * alt_vf_P[0][1] + alt_vf_P[1][1];
|
||||
const float P23 = -K2 * alt_vf_P[0][2] + alt_vf_P[1][2];
|
||||
const float P31 = -K3 * alt_vf_P[0][0] + alt_vf_P[2][0];
|
||||
const float P32 = -K3 * alt_vf_P[0][1] + alt_vf_P[2][1];
|
||||
const float P33 = -K3 * alt_vf_P[0][2] + alt_vf_P[2][2];
|
||||
|
||||
alt_vf_P[0][0] = P11;
|
||||
alt_vf_P[0][1] = P12;
|
||||
alt_vf_P[0][2] = P13;
|
||||
alt_vf_P[1][0] = P21;
|
||||
alt_vf_P[1][1] = P22;
|
||||
alt_vf_P[1][2] = P23;
|
||||
alt_vf_P[2][0] = P31;
|
||||
alt_vf_P[2][1] = P32;
|
||||
alt_vf_P[2][2] = P33;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
H = [0 1 0];
|
||||
R = 0.1;
|
||||
// state residual
|
||||
yd = vz - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*yd;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
void alt_vf_update_vz(float vz) {
|
||||
const float yd = vz - alt_vf_zdot;
|
||||
const float S = alt_vf_P[1][1] + R;
|
||||
const float K1 = alt_vf_P[0][1] * 1/S;
|
||||
const float K2 = alt_vf_P[1][1] * 1/S;
|
||||
const float K3 = alt_vf_P[2][1] * 1/S;
|
||||
|
||||
alt_vf_z = alt_vf_z + K1 * yd;
|
||||
alt_vf_zdot = alt_vf_zdot + K2 * yd;
|
||||
alt_vf_bias = alt_vf_bias + K3 * yd;
|
||||
|
||||
const float P11 = -K1 * alt_vf_P[1][0] + alt_vf_P[0][0];
|
||||
const float P12 = -K1 * alt_vf_P[1][1] + alt_vf_P[0][1];
|
||||
const float P13 = -K1 * alt_vf_P[1][2] + alt_vf_P[0][2];
|
||||
const float P21 = (1. - K2) * alt_vf_P[1][0];
|
||||
const float P22 = (1. - K2) * alt_vf_P[1][1];
|
||||
const float P23 = (1. - K2) * alt_vf_P[1][2];
|
||||
const float P31 = -K3 * alt_vf_P[1][0] + alt_vf_P[2][0];
|
||||
const float P32 = -K3 * alt_vf_P[1][1] + alt_vf_P[2][1];
|
||||
const float P33 = -K3 * alt_vf_P[1][2] + alt_vf_P[2][2];
|
||||
|
||||
alt_vf_P[0][0] = P11;
|
||||
alt_vf_P[0][1] = P12;
|
||||
alt_vf_P[0][2] = P13;
|
||||
alt_vf_P[1][0] = P21;
|
||||
alt_vf_P[1][1] = P22;
|
||||
alt_vf_P[1][2] = P23;
|
||||
alt_vf_P[2][0] = P31;
|
||||
alt_vf_P[2][1] = P32;
|
||||
alt_vf_P[2][2] = P33;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
#ifndef ALT_VFILTER_H
|
||||
#define ALT_VFILTER_H
|
||||
|
||||
#define STATE_SIZE 3
|
||||
|
||||
extern float alt_vf_z;
|
||||
extern float alt_vf_zdot;
|
||||
extern float alt_vf_bias;
|
||||
extern float alt_vf_P[STATE_SIZE][STATE_SIZE];
|
||||
|
||||
extern float alt_vf_z_meas;
|
||||
|
||||
extern void alt_vf_init(float z, float zdot, float bias);
|
||||
extern void alt_vf_predict(float accel);
|
||||
extern void alt_vf_update_z(float z_meas);
|
||||
extern void alt_vf_update_vz(float vz);
|
||||
|
||||
extern void alt_vf_periodic_task(void);
|
||||
|
||||
|
||||
#ifdef USE_BARO_MS5534A
|
||||
|
||||
#include "baro_MS5534A.h"
|
||||
|
||||
#define AltVFliterEvent() { \
|
||||
if (spi_message_received) { \
|
||||
/* Got a message on SPI. */ \
|
||||
spi_message_received = FALSE; \
|
||||
baro_MS5534A_event_task(); \
|
||||
if (baro_MS5534A_available) { \
|
||||
baro_MS5534A_available = FALSE; \
|
||||
baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; \
|
||||
if (alt_baro_enabled) { \
|
||||
alt_vf_update_z(-baro_MS5534A_z); \
|
||||
} \
|
||||
} \
|
||||
} \
|
||||
}
|
||||
|
||||
#else
|
||||
#define AltVFliterEvent() {}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* ALT_VFILTER_H */
|
||||
Reference in New Issue
Block a user