[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4575] add possibility to specify a measurement conf
From: |
Gautier Hattenberger |
Subject: |
[paparazzi-commits] [4575] add possibility to specify a measurement confidence in the vertical filter |
Date: |
Wed, 24 Feb 2010 09:54:56 +0000 |
Revision: 4575
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4575
Author: gautier
Date: 2010-02-24 09:54:56 +0000 (Wed, 24 Feb 2010)
Log Message:
-----------
add possibility to specify a measurement confidence in the vertical filter
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c
paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.h
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c 2010-02-24
09:54:23 UTC (rev 4574)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c 2010-02-24
09:54:56 UTC (rev 4575)
@@ -122,11 +122,11 @@
// 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) {
+ 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;
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.h 2010-02-24
09:54:23 UTC (rev 4574)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.h 2010-02-24
09:54:56 UTC (rev 4575)
@@ -37,6 +37,8 @@
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 */
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4575] add possibility to specify a measurement confidence in the vertical filter,
Gautier Hattenberger <=