[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4034] store hfilter state in struct.
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [4034] store hfilter state in struct. |
|
Date: |
Mon, 31 Aug 2009 18:46:46 +0000 |
Revision: 4034
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4034
Author: flixr
Date: 2009-08-31 18:46:44 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
store hfilter state in struct. Filter operates on state the "working"
pointer indicates.
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:46:26 UTC
(rev 4033)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:46:44 UTC
(rev 4034)
@@ -153,12 +153,12 @@
/* propagate horizontal filter */
b2_hff_propagate(x_accel_mean_f, y_accel_mean_f);
/* update ins state from horizontal filter */
- booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
- booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
- booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
- booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
- booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x);
- booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y);
+ booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+ booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+ booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+ booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+ booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
+ booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
}
}
} else {
@@ -216,12 +216,12 @@
#ifdef USE_HFF
b2_hff_update_gps();
- booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
- booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
- booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
- booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
- booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x);
- booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y);
+ booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+ booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+ booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+ booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+ booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
+ booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifndef USE_VFF /* only hf */
booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) /
INT32_POS_OF_CM_DEN;
booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z *
INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-08-31 18:46:26 UTC
(rev 4033)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-08-31 18:46:44 UTC
(rev 4034)
@@ -457,28 +457,28 @@
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \
&b2_hff_x_meas, \
- &b2_hff_xdotdot, \
- &b2_hff_x, \
- &b2_hff_xdot, \
- &b2_hff_xbias, \
- & b2_hff_xP[0][0], \
- & b2_hff_xP[1][1], \
- & b2_hff_xP[2][2]); \
+ &b2_hff_state.xdotdot, \
+ &b2_hff_state.x, \
+ &b2_hff_state.xdot, \
+ &b2_hff_state.xbias, \
+ &b2_hff_state.xP[0][0], \
+ &b2_hff_state.xP[1][1], \
+ &b2_hff_state.xP[2][2]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
&b2_hff_y_meas, \
- &b2_hff_ydotdot, \
- &b2_hff_y, \
- &b2_hff_ydot, \
- &b2_hff_ybias, \
- & b2_hff_yP[0][0],
\
- & b2_hff_yP[1][1], \
- & b2_hff_yP[2][2]); \
+ &b2_hff_state.ydotdot, \
+ &b2_hff_state.y, \
+ &b2_hff_state.ydot, \
+ &b2_hff_state.ybias, \
+ &b2_hff_state.yP[0][0],
\
+ &b2_hff_state.yP[1][1], \
+ &b2_hff_state.yP[2][2]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
- &lag_counter,
\
+
&b2_hff_save.lag_counter, \
&lag_counter_err,
\
&save_counter);
\
}
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-08-31
18:46:26 UTC (rev 4033)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-08-31
18:46:44 UTC (rev 4034)
@@ -50,20 +50,26 @@
#define Rpos 5.
#define Rspeed 1.
-/* filter states */
-float b2_hff_x;
-float b2_hff_xbias;
-float b2_hff_xdot;
-float b2_hff_xdotdot;
+/* output filter states */
+struct hfilter_f b2_hff_state;
+/* saved state when gps was valid */
+struct hfilter_f b2_hff_save;
+/* pointer to filter states to operate on */
+struct hfilter_f *b2_hff_work;
-float b2_hff_y;
-float b2_hff_ybias;
-float b2_hff_ydot;
-float b2_hff_ydotdot;
+/* float b2_hff_x; */
+/* float b2_hff_xbias; */
+/* float b2_hff_xdot; */
+/* float b2_hff_xdotdot; */
+/* float b2_hff_y; */
+/* float b2_hff_ybias; */
+/* float b2_hff_ydot; */
+/* float b2_hff_ydotdot; */
+
/* filter covariance matrices */
-float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
-float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+/* float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
+/* float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
#ifdef GPS_LAG
/* GPS_LAG is defined in seconds
@@ -73,16 +79,16 @@
#define GPS_LAG_N (int) (GPS_LAG * 512. / HFF_PRESCALER + 0.5)
/* state and covariance when GPS was valid */
-float b2_hff_x_sav;
-float b2_hff_xbias_sav;
-float b2_hff_xdot_sav;
-float b2_hff_xdotdot_sav;
-float b2_hff_y_sav;
-float b2_hff_ybias_sav;
-float b2_hff_ydot_sav;
-float b2_hff_ydotdot_sav;
-float b2_hff_xP_sav[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
-float b2_hff_yP_sav[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+/* float b2_hff_x_sav; */
+/* float b2_hff_xbias_sav; */
+/* float b2_hff_xdot_sav; */
+/* float b2_hff_xdotdot_sav; */
+/* float b2_hff_y_sav; */
+/* float b2_hff_ybias_sav; */
+/* float b2_hff_ydot_sav; */
+/* float b2_hff_ydotdot_sav; */
+/* float b2_hff_xP_sav[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
+/* float b2_hff_yP_sav[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
#define BUF_MAXN GPS_LAG_N+2
/* buffer with past mean accel values for redoing the propagation */
@@ -94,7 +100,7 @@
uint8_t buf_n; /* number of elements in buffer */
/* number of propagation steps since state was saved */
-uint8_t lag_counter;
+/* uint8_t lag_counter; */
/* by how many steps the estimated GPS validity point in time differed from
GPS_LAG_N */
int8_t lag_counter_err;
@@ -132,43 +138,45 @@
void b2_hff_init(float init_x, float init_xdot, float init_xbias, float
init_y, float init_ydot, float init_ybias) {
b2_hff_init_x(init_x, init_xdot, init_xbias);
b2_hff_init_y(init_y, init_ydot, init_ybias);
+ b2_hff_work = &b2_hff_state;
#ifdef GPS_LAG
buf_r = 0;
buf_w = 0;
buf_n = 0;
- lag_counter = 0;
+ b2_hff_save.lag_counter = 0;
+ b2_hff_state.lag_counter = GPS_LAG_N;
lag_counter_err = 0;
#endif
}
static inline void b2_hff_init_x(float init_x, float init_xdot, float
init_xbias) {
- b2_hff_x = init_x;
- b2_hff_xdot = init_xdot;
- b2_hff_xbias = init_xbias;
+ b2_hff_state.x = init_x;
+ b2_hff_state.xdot = init_xdot;
+ b2_hff_state.xbias = init_xbias;
int i, j;
for (i=0; i<B2_HFF_STATE_SIZE; i++) {
for (j=0; j<B2_HFF_STATE_SIZE; j++)
- b2_hff_xP[i][j] = 0.;
+ b2_hff_state.xP[i][j] = 0.;
if (i < 2)
- b2_hff_xP[i][i] = INIT_PXX;
+ b2_hff_state.xP[i][i] = INIT_PXX;
else
- b2_hff_xP[i][i] = INIT_PXX_BIAS;
+ b2_hff_state.xP[i][i] = INIT_PXX_BIAS;
}
}
static inline void b2_hff_init_y(float init_y, float init_ydot, float
init_ybias) {
- b2_hff_y = init_y;
- b2_hff_ydot = init_ydot;
- b2_hff_ybias = init_ybias;
+ b2_hff_state.y = init_y;
+ b2_hff_state.ydot = init_ydot;
+ b2_hff_state.ybias = init_ybias;
int i, j;
for (i=0; i<B2_HFF_STATE_SIZE; i++) {
for (j=0; j<B2_HFF_STATE_SIZE; j++)
- b2_hff_yP[i][j] = 0.;
+ b2_hff_state.yP[i][j] = 0.;
if (i < 2)
- b2_hff_yP[i][i] = INIT_PXX;
+ b2_hff_state.yP[i][i] = INIT_PXX;
else
- b2_hff_yP[i][i] = INIT_PXX_BIAS;
+ b2_hff_state.yP[i][i] = INIT_PXX_BIAS;
}
}
@@ -200,39 +208,58 @@
/* rollback the state and covariance matrix to time when last saved */
static inline void b2_hff_rollback_filter(void) {
- b2_hff_x = b2_hff_x_sav;
- b2_hff_xbias = b2_hff_xbias_sav;
- b2_hff_xdot = b2_hff_xdot_sav;
- b2_hff_xdotdot = b2_hff_xdotdot_sav;
- b2_hff_y = b2_hff_y_sav;
- b2_hff_ybias = b2_hff_ybias_sav;
- b2_hff_ydot = b2_hff_ydot_sav;
- b2_hff_ydotdot = b2_hff_ydotdot_sav;
+ /* b2_hff_x = b2_hff_x_sav; */
+/* b2_hff_xbias = b2_hff_xbias_sav; */
+/* b2_hff_xdot = b2_hff_xdot_sav; */
+/* b2_hff_xdotdot = b2_hff_xdotdot_sav; */
+/* b2_hff_y = b2_hff_y_sav; */
+/* b2_hff_ybias = b2_hff_ybias_sav; */
+/* b2_hff_ydot = b2_hff_ydot_sav; */
+/* b2_hff_ydotdot = b2_hff_ydotdot_sav; */
+/* for (int i=0; i < B2_HFF_STATE_SIZE; i++) { */
+/* for (int j=0; j < B2_HFF_STATE_SIZE; j++) { */
+/* b2_hff_xP[i][j] = b2_hff_xP_sav[i][j]; */
+/* b2_hff_yP[i][j] = b2_hff_yP_sav[i][j]; */
+/* } */
+/* } */
+ b2_hff_work = &b2_hff_save;
+}
+
+/* save current state for later rollback */
+static inline void b2_hff_save_filter(void) {
+ b2_hff_save.x = b2_hff_state.x;
+ b2_hff_save.xbias = b2_hff_state.xbias;
+ b2_hff_save.xdot = b2_hff_state.xdot;
+ b2_hff_save.xdotdot = b2_hff_state.xdotdot;
+ b2_hff_save.y = b2_hff_state.y;
+ b2_hff_save.ybias = b2_hff_state.ybias;
+ b2_hff_save.ydot = b2_hff_state.ydot;
+ b2_hff_save.ydotdot = b2_hff_state.ydotdot;
for (int i=0; i < B2_HFF_STATE_SIZE; i++) {
for (int j=0; j < B2_HFF_STATE_SIZE; j++) {
- b2_hff_xP[i][j] = b2_hff_xP_sav[i][j];
- b2_hff_yP[i][j] = b2_hff_yP_sav[i][j];
+ b2_hff_save.xP[i][j] = b2_hff_state.xP[i][j];
+ b2_hff_save.yP[i][j] = b2_hff_state.yP[i][j];
}
}
+ b2_hff_save.lag_counter = 0;
}
-/* save current state for later rollback */
-static inline void b2_hff_save_filter(void) {
- b2_hff_x_sav = b2_hff_x;
- b2_hff_xbias_sav = b2_hff_xbias;
- b2_hff_xdot_sav = b2_hff_xdot;
- b2_hff_xdotdot_sav = b2_hff_xdotdot;
- b2_hff_y_sav = b2_hff_y;
- b2_hff_ybias_sav = b2_hff_ybias;
- b2_hff_ydot_sav = b2_hff_ydot;
- b2_hff_ydotdot_sav = b2_hff_ydotdot;
+/* copy working state to output state */
+static inline void b2_hff_set_state(struct hfilter_f* source) {
+ b2_hff_state.x = source->x;
+ b2_hff_state.xbias = source->xbias;
+ b2_hff_state.xdot = source->xdot;
+ b2_hff_state.xdotdot = source->xdotdot;
+ b2_hff_state.y = source->y;
+ b2_hff_state.ybias = source->ybias;
+ b2_hff_state.ydot = source->ydot;
+ b2_hff_state.ydotdot = source->ydotdot;
for (int i=0; i < B2_HFF_STATE_SIZE; i++) {
for (int j=0; j < B2_HFF_STATE_SIZE; j++) {
- b2_hff_xP_sav[i][j] = b2_hff_xP[i][j];
- b2_hff_yP_sav[i][j] = b2_hff_yP[i][j];
+ b2_hff_state.xP[i][j] = source->xP[i][j];
+ b2_hff_state.yP[i][j] = source->yP[i][j];
}
}
- lag_counter = 0;
}
#endif
@@ -270,71 +297,71 @@
b2_hff_propagate_y();
#ifdef GPS_LAG
- if (lag_counter < 2*GPS_LAG_N) // prevent from overflowing if no gps
available
- lag_counter++;
+ if (b2_hff_save.lag_counter < 2*GPS_LAG_N) // prevent from overflowing if no
gps available
+ b2_hff_save.lag_counter++;
#endif
}
static inline void b2_hff_propagate_x(void) {
/* update state */
- b2_hff_xdotdot = b2_hff_xaccel - b2_hff_xbias;
- b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot + DT_HFILTER * DT_HFILTER / 2
* b2_hff_xdotdot;
- b2_hff_xdot = b2_hff_xdot + DT_HFILTER * b2_hff_xdotdot;
+ b2_hff_work->xdotdot = b2_hff_xaccel - b2_hff_work->xbias;
+ b2_hff_work->x = b2_hff_work->x + DT_HFILTER * b2_hff_work->xdot +
DT_HFILTER * DT_HFILTER / 2 * b2_hff_work->xdotdot;
+ b2_hff_work->xdot = b2_hff_work->xdot + DT_HFILTER * b2_hff_work->xdotdot;
/* update covariance */
- const float FPF00 = b2_hff_xP[0][0] + DT_HFILTER * ( b2_hff_xP[1][0] +
b2_hff_xP[0][1] + DT_HFILTER * b2_hff_xP[1][1] );
- const float FPF01 = b2_hff_xP[0][1] + DT_HFILTER * ( b2_hff_xP[1][1] -
b2_hff_xP[0][2] - DT_HFILTER * b2_hff_xP[1][2] );
- const float FPF02 = b2_hff_xP[0][2] + DT_HFILTER * ( b2_hff_xP[1][2] );
- const float FPF10 = b2_hff_xP[1][0] + DT_HFILTER * (-b2_hff_xP[2][0] +
b2_hff_xP[1][1] - DT_HFILTER * b2_hff_xP[2][1] );
- const float FPF11 = b2_hff_xP[1][1] + DT_HFILTER * (-b2_hff_xP[2][1] -
b2_hff_xP[1][2] + DT_HFILTER * b2_hff_xP[2][2] );
- const float FPF12 = b2_hff_xP[1][2] + DT_HFILTER * (-b2_hff_xP[2][2] );
- const float FPF20 = b2_hff_xP[2][0] + DT_HFILTER * ( b2_hff_xP[2][1] );
- const float FPF21 = b2_hff_xP[2][1] + DT_HFILTER * (-b2_hff_xP[2][2] );
- const float FPF22 = b2_hff_xP[2][2];
+ const float FPF00 = b2_hff_work->xP[0][0] + DT_HFILTER * (
b2_hff_work->xP[1][0] + b2_hff_work->xP[0][1] + DT_HFILTER *
b2_hff_work->xP[1][1] );
+ const float FPF01 = b2_hff_work->xP[0][1] + DT_HFILTER * (
b2_hff_work->xP[1][1] - b2_hff_work->xP[0][2] - DT_HFILTER *
b2_hff_work->xP[1][2] );
+ const float FPF02 = b2_hff_work->xP[0][2] + DT_HFILTER * (
b2_hff_work->xP[1][2] );
+ const float FPF10 = b2_hff_work->xP[1][0] + DT_HFILTER *
(-b2_hff_work->xP[2][0] + b2_hff_work->xP[1][1] - DT_HFILTER *
b2_hff_work->xP[2][1] );
+ const float FPF11 = b2_hff_work->xP[1][1] + DT_HFILTER *
(-b2_hff_work->xP[2][1] - b2_hff_work->xP[1][2] + DT_HFILTER *
b2_hff_work->xP[2][2] );
+ const float FPF12 = b2_hff_work->xP[1][2] + DT_HFILTER *
(-b2_hff_work->xP[2][2] );
+ const float FPF20 = b2_hff_work->xP[2][0] + DT_HFILTER * (
b2_hff_work->xP[2][1] );
+ const float FPF21 = b2_hff_work->xP[2][1] + DT_HFILTER *
(-b2_hff_work->xP[2][2] );
+ const float FPF22 = b2_hff_work->xP[2][2];
- b2_hff_xP[0][0] = FPF00 + Q;
- b2_hff_xP[0][1] = FPF01;
- b2_hff_xP[0][2] = FPF02;
- b2_hff_xP[1][0] = FPF10;
- b2_hff_xP[1][1] = FPF11 + Qdotdot;
- b2_hff_xP[1][2] = FPF12;
- b2_hff_xP[2][0] = FPF20;
- b2_hff_xP[2][1] = FPF21;
- b2_hff_xP[2][2] = FPF22 + Qbiasbias;
+ b2_hff_work->xP[0][0] = FPF00 + Q;
+ b2_hff_work->xP[0][1] = FPF01;
+ b2_hff_work->xP[0][2] = FPF02;
+ b2_hff_work->xP[1][0] = FPF10;
+ b2_hff_work->xP[1][1] = FPF11 + Qdotdot;
+ b2_hff_work->xP[1][2] = FPF12;
+ b2_hff_work->xP[2][0] = FPF20;
+ b2_hff_work->xP[2][1] = FPF21;
+ b2_hff_work->xP[2][2] = FPF22 + Qbiasbias;
}
static inline void b2_hff_propagate_y(void) {
/* update state */
- b2_hff_ydotdot = b2_hff_yaccel - b2_hff_ybias;
- b2_hff_y = b2_hff_y + DT_HFILTER * b2_hff_ydot;
- b2_hff_ydot = b2_hff_ydot + DT_HFILTER * b2_hff_ydotdot;
+ b2_hff_work->ydotdot = b2_hff_yaccel - b2_hff_work->ybias;
+ b2_hff_work->y = b2_hff_work->y + DT_HFILTER * b2_hff_work->ydot;
+ b2_hff_work->ydot = b2_hff_work->ydot + DT_HFILTER * b2_hff_work->ydotdot;
/* update covariance */
- const float FPF00 = b2_hff_yP[0][0] + DT_HFILTER * ( b2_hff_yP[1][0] +
b2_hff_yP[0][1] + DT_HFILTER * b2_hff_yP[1][1] );
- const float FPF01 = b2_hff_yP[0][1] + DT_HFILTER * ( b2_hff_yP[1][1] -
b2_hff_yP[0][2] - DT_HFILTER * b2_hff_yP[1][2] );
- const float FPF02 = b2_hff_yP[0][2] + DT_HFILTER * ( b2_hff_yP[1][2] );
- const float FPF10 = b2_hff_yP[1][0] + DT_HFILTER * (-b2_hff_yP[2][0] +
b2_hff_yP[1][1] - DT_HFILTER * b2_hff_yP[2][1] );
- const float FPF11 = b2_hff_yP[1][1] + DT_HFILTER * (-b2_hff_yP[2][1] -
b2_hff_yP[1][2] + DT_HFILTER * b2_hff_yP[2][2] );
- const float FPF12 = b2_hff_yP[1][2] + DT_HFILTER * (-b2_hff_yP[2][2] );
- const float FPF20 = b2_hff_yP[2][0] + DT_HFILTER * ( b2_hff_yP[2][1] );
- const float FPF21 = b2_hff_yP[2][1] + DT_HFILTER * (-b2_hff_yP[2][2] );
- const float FPF22 = b2_hff_yP[2][2];
+ const float FPF00 = b2_hff_work->yP[0][0] + DT_HFILTER * (
b2_hff_work->yP[1][0] + b2_hff_work->yP[0][1] + DT_HFILTER *
b2_hff_work->yP[1][1] );
+ const float FPF01 = b2_hff_work->yP[0][1] + DT_HFILTER * (
b2_hff_work->yP[1][1] - b2_hff_work->yP[0][2] - DT_HFILTER *
b2_hff_work->yP[1][2] );
+ const float FPF02 = b2_hff_work->yP[0][2] + DT_HFILTER * (
b2_hff_work->yP[1][2] );
+ const float FPF10 = b2_hff_work->yP[1][0] + DT_HFILTER *
(-b2_hff_work->yP[2][0] + b2_hff_work->yP[1][1] - DT_HFILTER *
b2_hff_work->yP[2][1] );
+ const float FPF11 = b2_hff_work->yP[1][1] + DT_HFILTER *
(-b2_hff_work->yP[2][1] - b2_hff_work->yP[1][2] + DT_HFILTER *
b2_hff_work->yP[2][2] );
+ const float FPF12 = b2_hff_work->yP[1][2] + DT_HFILTER *
(-b2_hff_work->yP[2][2] );
+ const float FPF20 = b2_hff_work->yP[2][0] + DT_HFILTER * (
b2_hff_work->yP[2][1] );
+ const float FPF21 = b2_hff_work->yP[2][1] + DT_HFILTER *
(-b2_hff_work->yP[2][2] );
+ const float FPF22 = b2_hff_work->yP[2][2];
- b2_hff_yP[0][0] = FPF00 + Q;
- b2_hff_yP[0][1] = FPF01;
- b2_hff_yP[0][2] = FPF02;
- b2_hff_yP[1][0] = FPF10;
- b2_hff_yP[1][1] = FPF11 + Qdotdot;
- b2_hff_yP[1][2] = FPF12;
- b2_hff_yP[2][0] = FPF20;
- b2_hff_yP[2][1] = FPF21;
- b2_hff_yP[2][2] = FPF22 + Qbiasbias;
+ b2_hff_work->yP[0][0] = FPF00 + Q;
+ b2_hff_work->yP[0][1] = FPF01;
+ b2_hff_work->yP[0][2] = FPF02;
+ b2_hff_work->yP[1][0] = FPF10;
+ b2_hff_work->yP[1][1] = FPF11 + Qdotdot;
+ b2_hff_work->yP[1][2] = FPF12;
+ b2_hff_work->yP[2][0] = FPF20;
+ b2_hff_work->yP[2][1] = FPF21;
+ b2_hff_work->yP[2][2] = FPF22 + Qbiasbias;
}
void b2_hff_update_gps(void) {
#ifdef GPS_LAG
- lag_counter_err = lag_counter - GPS_LAG_N;
+ lag_counter_err = b2_hff_save.lag_counter - GPS_LAG_N;
/* roll back if state was saved approx when GPS was valid */
if (abs(lag_counter_err) < 3) {
b2_hff_rollback_filter();
@@ -354,11 +381,13 @@
/* roll back if state was saved approx when GPS was valid */
if (abs(lag_counter_err) < 3) {
/* redo all propagation steps since GPS update */
- for (int i=lag_counter-1; i >= 0; i--) {
+ for (int i=b2_hff_save.lag_counter-1; i >= 0; i--) {
b2_hff_get_past_accel(i);
b2_hff_propagate_x();
b2_hff_propagate_y();
}
+ b2_hff_set_state(b2_hff_work);
+ b2_hff_work = &b2_hff_state;
}
/* reset save counter */
@@ -389,70 +418,70 @@
static inline void b2_hff_update_x(float x_meas) {
b2_hff_x_meas = x_meas;
- const float y = x_meas - b2_hff_x;
- const float S = b2_hff_xP[0][0] + Rpos;
- const float K1 = b2_hff_xP[0][0] * 1/S;
- const float K2 = b2_hff_xP[1][0] * 1/S;
- const float K3 = b2_hff_xP[2][0] * 1/S;
+ const float y = x_meas - b2_hff_work->x;
+ const float S = b2_hff_work->xP[0][0] + Rpos;
+ const float K1 = b2_hff_work->xP[0][0] * 1/S;
+ const float K2 = b2_hff_work->xP[1][0] * 1/S;
+ const float K3 = b2_hff_work->xP[2][0] * 1/S;
- b2_hff_x = b2_hff_x + K1 * y;
- b2_hff_xdot = b2_hff_xdot + K2 * y;
- b2_hff_xbias = b2_hff_xbias + K3 * y;
+ b2_hff_work->x = b2_hff_work->x + K1 * y;
+ b2_hff_work->xdot = b2_hff_work->xdot + K2 * y;
+ b2_hff_work->xbias = b2_hff_work->xbias + K3 * y;
- const float P11 = (1. - K1) * b2_hff_xP[0][0];
- const float P12 = (1. - K1) * b2_hff_xP[0][1];
- const float P13 = (1. - K1) * b2_hff_xP[0][2];
- const float P21 = -K2 * b2_hff_xP[0][0] + b2_hff_xP[1][0];
- const float P22 = -K2 * b2_hff_xP[0][1] + b2_hff_xP[1][1];
- const float P23 = -K2 * b2_hff_xP[0][2] + b2_hff_xP[1][2];
- const float P31 = -K3 * b2_hff_xP[0][0] + b2_hff_xP[2][0];
- const float P32 = -K3 * b2_hff_xP[0][1] + b2_hff_xP[2][1];
- const float P33 = -K3 * b2_hff_xP[0][2] + b2_hff_xP[2][2];
+ const float P11 = (1. - K1) * b2_hff_work->xP[0][0];
+ const float P12 = (1. - K1) * b2_hff_work->xP[0][1];
+ const float P13 = (1. - K1) * b2_hff_work->xP[0][2];
+ const float P21 = -K2 * b2_hff_work->xP[0][0] + b2_hff_work->xP[1][0];
+ const float P22 = -K2 * b2_hff_work->xP[0][1] + b2_hff_work->xP[1][1];
+ const float P23 = -K2 * b2_hff_work->xP[0][2] + b2_hff_work->xP[1][2];
+ const float P31 = -K3 * b2_hff_work->xP[0][0] + b2_hff_work->xP[2][0];
+ const float P32 = -K3 * b2_hff_work->xP[0][1] + b2_hff_work->xP[2][1];
+ const float P33 = -K3 * b2_hff_work->xP[0][2] + b2_hff_work->xP[2][2];
- b2_hff_xP[0][0] = P11;
- b2_hff_xP[0][1] = P12;
- b2_hff_xP[0][2] = P13;
- b2_hff_xP[1][0] = P21;
- b2_hff_xP[1][1] = P22;
- b2_hff_xP[1][2] = P23;
- b2_hff_xP[2][0] = P31;
- b2_hff_xP[2][1] = P32;
- b2_hff_xP[2][2] = P33;
+ b2_hff_work->xP[0][0] = P11;
+ b2_hff_work->xP[0][1] = P12;
+ b2_hff_work->xP[0][2] = P13;
+ b2_hff_work->xP[1][0] = P21;
+ b2_hff_work->xP[1][1] = P22;
+ b2_hff_work->xP[1][2] = P23;
+ b2_hff_work->xP[2][0] = P31;
+ b2_hff_work->xP[2][1] = P32;
+ b2_hff_work->xP[2][2] = P33;
}
static inline void b2_hff_update_y(float y_meas) {
b2_hff_y_meas = y_meas;
- const float y = y_meas - b2_hff_y;
- const float S = b2_hff_yP[0][0] + Rpos;
- const float K1 = b2_hff_yP[0][0] * 1/S;
- const float K2 = b2_hff_yP[1][0] * 1/S;
- const float K3 = b2_hff_yP[2][0] * 1/S;
+ const float y = y_meas - b2_hff_work->y;
+ const float S = b2_hff_work->yP[0][0] + Rpos;
+ const float K1 = b2_hff_work->yP[0][0] * 1/S;
+ const float K2 = b2_hff_work->yP[1][0] * 1/S;
+ const float K3 = b2_hff_work->yP[2][0] * 1/S;
- b2_hff_y = b2_hff_y + K1 * y;
- b2_hff_ydot = b2_hff_ydot + K2 * y;
- b2_hff_ybias = b2_hff_ybias + K3 * y;
+ b2_hff_work->y = b2_hff_work->y + K1 * y;
+ b2_hff_work->ydot = b2_hff_work->ydot + K2 * y;
+ b2_hff_work->ybias = b2_hff_work->ybias + K3 * y;
- const float P11 = (1. - K1) * b2_hff_yP[0][0];
- const float P12 = (1. - K1) * b2_hff_yP[0][1];
- const float P13 = (1. - K1) * b2_hff_yP[0][2];
- const float P21 = -K2 * b2_hff_yP[0][0] + b2_hff_yP[1][0];
- const float P22 = -K2 * b2_hff_yP[0][1] + b2_hff_yP[1][1];
- const float P23 = -K2 * b2_hff_yP[0][2] + b2_hff_yP[1][2];
- const float P31 = -K3 * b2_hff_yP[0][0] + b2_hff_yP[2][0];
- const float P32 = -K3 * b2_hff_yP[0][1] + b2_hff_yP[2][1];
- const float P33 = -K3 * b2_hff_yP[0][2] + b2_hff_yP[2][2];
+ const float P11 = (1. - K1) * b2_hff_work->yP[0][0];
+ const float P12 = (1. - K1) * b2_hff_work->yP[0][1];
+ const float P13 = (1. - K1) * b2_hff_work->yP[0][2];
+ const float P21 = -K2 * b2_hff_work->yP[0][0] + b2_hff_work->yP[1][0];
+ const float P22 = -K2 * b2_hff_work->yP[0][1] + b2_hff_work->yP[1][1];
+ const float P23 = -K2 * b2_hff_work->yP[0][2] + b2_hff_work->yP[1][2];
+ const float P31 = -K3 * b2_hff_work->yP[0][0] + b2_hff_work->yP[2][0];
+ const float P32 = -K3 * b2_hff_work->yP[0][1] + b2_hff_work->yP[2][1];
+ const float P33 = -K3 * b2_hff_work->yP[0][2] + b2_hff_work->yP[2][2];
- b2_hff_yP[0][0] = P11;
- b2_hff_yP[0][1] = P12;
- b2_hff_yP[0][2] = P13;
- b2_hff_yP[1][0] = P21;
- b2_hff_yP[1][1] = P22;
- b2_hff_yP[1][2] = P23;
- b2_hff_yP[2][0] = P31;
- b2_hff_yP[2][1] = P32;
- b2_hff_yP[2][2] = P33;
+ b2_hff_work->yP[0][0] = P11;
+ b2_hff_work->yP[0][1] = P12;
+ b2_hff_work->yP[0][2] = P13;
+ b2_hff_work->yP[1][0] = P21;
+ b2_hff_work->yP[1][1] = P22;
+ b2_hff_work->yP[1][2] = P23;
+ b2_hff_work->yP[2][0] = P31;
+ b2_hff_work->yP[2][1] = P32;
+ b2_hff_work->yP[2][2] = P33;
}
@@ -479,68 +508,68 @@
}
static inline void b2_hff_update_xdot(float v) {
- const float yd = v - b2_hff_xdot;
- const float S = b2_hff_xP[1][1] + Rspeed;
- const float K1 = b2_hff_xP[0][1] * 1/S;
- const float K2 = b2_hff_xP[1][1] * 1/S;
- const float K3 = b2_hff_xP[2][1] * 1/S;
+ const float yd = v - b2_hff_work->xdot;
+ const float S = b2_hff_work->xP[1][1] + Rspeed;
+ const float K1 = b2_hff_work->xP[0][1] * 1/S;
+ const float K2 = b2_hff_work->xP[1][1] * 1/S;
+ const float K3 = b2_hff_work->xP[2][1] * 1/S;
- b2_hff_x = b2_hff_x + K1 * yd;
- b2_hff_xdot = b2_hff_xdot + K2 * yd;
- b2_hff_xbias = b2_hff_xbias + K3 * yd;
+ b2_hff_work->x = b2_hff_work->x + K1 * yd;
+ b2_hff_work->xdot = b2_hff_work->xdot + K2 * yd;
+ b2_hff_work->xbias = b2_hff_work->xbias + K3 * yd;
- const float P11 = -K1 * b2_hff_xP[1][0] + b2_hff_xP[0][0];
- const float P12 = -K1 * b2_hff_xP[1][1] + b2_hff_xP[0][1];
- const float P13 = -K1 * b2_hff_xP[1][2] + b2_hff_xP[0][2];
- const float P21 = (1. - K2) * b2_hff_xP[1][0];
- const float P22 = (1. - K2) * b2_hff_xP[1][1];
- const float P23 = (1. - K2) * b2_hff_xP[1][2];
- const float P31 = -K3 * b2_hff_xP[1][0] + b2_hff_xP[2][0];
- const float P32 = -K3 * b2_hff_xP[1][1] + b2_hff_xP[2][1];
- const float P33 = -K3 * b2_hff_xP[1][2] + b2_hff_xP[2][2];
+ const float P11 = -K1 * b2_hff_work->xP[1][0] + b2_hff_work->xP[0][0];
+ const float P12 = -K1 * b2_hff_work->xP[1][1] + b2_hff_work->xP[0][1];
+ const float P13 = -K1 * b2_hff_work->xP[1][2] + b2_hff_work->xP[0][2];
+ const float P21 = (1. - K2) * b2_hff_work->xP[1][0];
+ const float P22 = (1. - K2) * b2_hff_work->xP[1][1];
+ const float P23 = (1. - K2) * b2_hff_work->xP[1][2];
+ const float P31 = -K3 * b2_hff_work->xP[1][0] + b2_hff_work->xP[2][0];
+ const float P32 = -K3 * b2_hff_work->xP[1][1] + b2_hff_work->xP[2][1];
+ const float P33 = -K3 * b2_hff_work->xP[1][2] + b2_hff_work->xP[2][2];
- b2_hff_xP[0][0] = P11;
- b2_hff_xP[0][1] = P12;
- b2_hff_xP[0][2] = P13;
- b2_hff_xP[1][0] = P21;
- b2_hff_xP[1][1] = P22;
- b2_hff_xP[1][2] = P23;
- b2_hff_xP[2][0] = P31;
- b2_hff_xP[2][1] = P32;
- b2_hff_xP[2][2] = P33;
+ b2_hff_work->xP[0][0] = P11;
+ b2_hff_work->xP[0][1] = P12;
+ b2_hff_work->xP[0][2] = P13;
+ b2_hff_work->xP[1][0] = P21;
+ b2_hff_work->xP[1][1] = P22;
+ b2_hff_work->xP[1][2] = P23;
+ b2_hff_work->xP[2][0] = P31;
+ b2_hff_work->xP[2][1] = P32;
+ b2_hff_work->xP[2][2] = P33;
}
static inline void b2_hff_update_ydot(float v) {
- const float yd = v - b2_hff_ydot;
- const float S = b2_hff_yP[1][1] + Rspeed;
- const float K1 = b2_hff_yP[0][1] * 1/S;
- const float K2 = b2_hff_yP[1][1] * 1/S;
- const float K3 = b2_hff_yP[2][1] * 1/S;
+ const float yd = v - b2_hff_work->ydot;
+ const float S = b2_hff_work->yP[1][1] + Rspeed;
+ const float K1 = b2_hff_work->yP[0][1] * 1/S;
+ const float K2 = b2_hff_work->yP[1][1] * 1/S;
+ const float K3 = b2_hff_work->yP[2][1] * 1/S;
- b2_hff_y = b2_hff_y + K1 * yd;
- b2_hff_ydot = b2_hff_ydot + K2 * yd;
- b2_hff_ybias = b2_hff_ybias + K3 * yd;
+ b2_hff_work->y = b2_hff_work->y + K1 * yd;
+ b2_hff_work->ydot = b2_hff_work->ydot + K2 * yd;
+ b2_hff_work->ybias = b2_hff_work->ybias + K3 * yd;
- const float P11 = -K1 * b2_hff_yP[1][0] + b2_hff_yP[0][0];
- const float P12 = -K1 * b2_hff_yP[1][1] + b2_hff_yP[0][1];
- const float P13 = -K1 * b2_hff_yP[1][2] + b2_hff_yP[0][2];
- const float P21 = (1. - K2) * b2_hff_yP[1][0];
- const float P22 = (1. - K2) * b2_hff_yP[1][1];
- const float P23 = (1. - K2) * b2_hff_yP[1][2];
- const float P31 = -K3 * b2_hff_yP[1][0] + b2_hff_yP[2][0];
- const float P32 = -K3 * b2_hff_yP[1][1] + b2_hff_yP[2][1];
- const float P33 = -K3 * b2_hff_yP[1][2] + b2_hff_yP[2][2];
+ const float P11 = -K1 * b2_hff_work->yP[1][0] + b2_hff_work->yP[0][0];
+ const float P12 = -K1 * b2_hff_work->yP[1][1] + b2_hff_work->yP[0][1];
+ const float P13 = -K1 * b2_hff_work->yP[1][2] + b2_hff_work->yP[0][2];
+ const float P21 = (1. - K2) * b2_hff_work->yP[1][0];
+ const float P22 = (1. - K2) * b2_hff_work->yP[1][1];
+ const float P23 = (1. - K2) * b2_hff_work->yP[1][2];
+ const float P31 = -K3 * b2_hff_work->yP[1][0] + b2_hff_work->yP[2][0];
+ const float P32 = -K3 * b2_hff_work->yP[1][1] + b2_hff_work->yP[2][1];
+ const float P33 = -K3 * b2_hff_work->yP[1][2] + b2_hff_work->yP[2][2];
- b2_hff_yP[0][0] = P11;
- b2_hff_yP[0][1] = P12;
- b2_hff_yP[0][2] = P13;
- b2_hff_yP[1][0] = P21;
- b2_hff_yP[1][1] = P22;
- b2_hff_yP[1][2] = P23;
- b2_hff_yP[2][0] = P31;
- b2_hff_yP[2][1] = P32;
- b2_hff_yP[2][2] = P33;
+ b2_hff_work->yP[0][0] = P11;
+ b2_hff_work->yP[0][1] = P12;
+ b2_hff_work->yP[0][2] = P13;
+ b2_hff_work->yP[1][0] = P21;
+ b2_hff_work->yP[1][1] = P22;
+ b2_hff_work->yP[1][2] = P23;
+ b2_hff_work->yP[2][0] = P31;
+ b2_hff_work->yP[2][1] = P32;
+ b2_hff_work->yP[2][2] = P33;
}
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h 2009-08-31
18:46:26 UTC (rev 4033)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h 2009-08-31
18:46:44 UTC (rev 4034)
@@ -35,19 +35,37 @@
#define B2_HFF_UPDATE_SPEED
#define B2_HFF_UPDATE_POS
-extern float b2_hff_x;
-extern float b2_hff_xbias;
-extern float b2_hff_xdot;
-extern float b2_hff_xdotdot;
+struct hfilter_f {
+ float x;
+ float xbias;
+ float xdot;
+ float xdotdot;
+ float y;
+ float ybias;
+ float ydot;
+ float ydotdot;
+ float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+ float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+ uint8_t lag_counter;
+};
-extern float b2_hff_y;
-extern float b2_hff_ybias;
-extern float b2_hff_ydot;
-extern float b2_hff_ydotdot;
+extern struct hfilter_f b2_hff_state;
+extern struct hfilter_f b2_hff_save;
+extern struct hfilter_f *b2_hff_work;
-extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
-extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+/* extern float b2_hff_x; */
+/* extern float b2_hff_xbias; */
+/* extern float b2_hff_xdot; */
+/* extern float b2_hff_xdotdot; */
+/* extern float b2_hff_y; */
+/* extern float b2_hff_ybias; */
+/* extern float b2_hff_ydot; */
+/* extern float b2_hff_ydotdot; */
+
+/* extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
+/* extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
+
extern float b2_hff_x_meas;
extern float b2_hff_y_meas;
@@ -59,7 +77,7 @@
#ifdef GPS_LAG
extern void b2_hff_store_accel(float x, float y);
-extern uint8_t lag_counter;
+/* extern uint8_t lag_counter; */
extern int8_t lag_counter_err;
extern int8_t save_counter;
#endif
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4034] store hfilter state in struct.,
Felix Ruess <=