Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

add possibility to specify a measurement confidence in the vertical filter

Browse Source
This commit is contained in:
Gautier Hattenberger
2010-02-24 09:54:56 +00:00
parent 26ac86a693
commit e326d5481c
2 changed files with 63 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/ins/booz2_vf_float.c
+61 -2
View File
@@ -122,11 +122,11 @@ void b2_vff_propagate(float accel) {
// update covariance
Pp = Pm - K*H*Pm;
*/
void b2_vff_update(float z_meas) {
static inline void update_z_conf(float z_meas, float conf) {
b2_vff_z_meas = z_meas;
const float y = z_meas - b2_vff_z;
const float S = b2_vff_P[0][0] + R;
const float S = b2_vff_P[0][0] + conf;
const float K1 = b2_vff_P[0][0] * 1/S;
const float K2 = b2_vff_P[1][0] * 1/S;
const float K3 = b2_vff_P[2][0] * 1/S;
@@ -157,6 +157,65 @@ void b2_vff_update(float z_meas) {
}
void b2_vff_update(float z_meas) {
update_z_conf(z_meas, R);
}
void b2_vff_update_z_conf(float z_meas, float conf) {
update_z_conf(z_meas, conf);
}
/*
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;
*/
static inline void update_vz_conf(float vz, float conf) {
const float yd = vz - b2_vff_zdot;
const float S = b2_vff_P[1][1] + conf;
const float K1 = b2_vff_P[0][1] * 1/S;
const float K2 = b2_vff_P[1][1] * 1/S;
const float K3 = b2_vff_P[2][1] * 1/S;
b2_vff_z = b2_vff_z + K1 * yd;
b2_vff_zdot = b2_vff_zdot + K2 * yd;
b2_vff_bias = b2_vff_bias + K3 * yd;
const float P11 = -K1 * b2_vff_P[1][0] + b2_vff_P[0][0];
const float P12 = -K1 * b2_vff_P[1][1] + b2_vff_P[0][1];
const float P13 = -K1 * b2_vff_P[1][2] + b2_vff_P[0][2];
const float P21 = (1. - K2) * b2_vff_P[1][0];
const float P22 = (1. - K2) * b2_vff_P[1][1];
const float P23 = (1. - K2) * b2_vff_P[1][2];
const float P31 = -K3 * b2_vff_P[1][0] + b2_vff_P[2][0];
const float P32 = -K3 * b2_vff_P[1][1] + b2_vff_P[2][1];
const float P33 = -K3 * b2_vff_P[1][2] + b2_vff_P[2][2];
b2_vff_P[0][0] = P11;
b2_vff_P[0][1] = P12;
b2_vff_P[0][2] = P13;
b2_vff_P[1][0] = P21;
b2_vff_P[1][1] = P22;
b2_vff_P[1][2] = P23;
b2_vff_P[2][0] = P31;
b2_vff_P[2][1] = P32;
b2_vff_P[2][2] = P33;
}
void b2_vff_update_vz_conf(float vz_meas, float conf) {
update_vz_conf(vz_meas, conf);
}
void b2_vff_realign(float z_meas) {
b2_vff_z = z_meas;
b2_vff_zdot = 0;
sw/airborne/booz/ins/booz2_vf_float.h
+2
View File
@@ -37,6 +37,8 @@ extern float b2_vff_z_meas;
extern void b2_vff_init(float z, float zdot, float bias);
extern void b2_vff_propagate(float accel);
extern void b2_vff_update(float z_meas);
extern void b2_vff_update_z_conf(float z_meas, float conf);
extern void b2_vff_update_vz_conf(float vz_meas, float conf);
extern void b2_vff_realign(float z_meas);
#endif /* BOOZ2_VF_FLOAT_H */