diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index d07be55acd..62a54a06f2 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -203,8 +203,9 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
- #enable horizontal filter
- ap.CFLAGS += -DUSE_HFF -DHFF_PRESCALER=16 -DGPS_LAG=0.2
+ include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+ #set GPS lag for horizontal filter
+ ap.CFLAGS += -DGPS_LAG=0.8
diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile
index 5a0140773d..860bd0eece 100644
--- a/conf/autopilot/booz2_autopilot.makefile
+++ b/conf/autopilot/booz2_autopilot.makefile
@@ -130,8 +130,15 @@ ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
ap.srcs += $(SRC_BOOZ)/booz2_ins.c
ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
-# horizontal filter float version
-ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
+
+#
+# INS choice
+#
+# include booz2_ins_hff.makefile
+# or
+# nothing
+#
+
# vertical filter float version
ap.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile
index 48e2175800..2f6dee7c81 100644
--- a/conf/autopilot/booz2_simulator_nps.makefile
+++ b/conf/autopilot/booz2_simulator_nps.makefile
@@ -138,8 +138,14 @@ sim.srcs += $(SRC_BOOZ)/booz2_ins.c
# vertical filter float version
sim.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
sim.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)"
-sim.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
+#
+# INS choice
+#
+# include booz2_ins_hff.makefile
+# or
+# nothing
+#
sim.srcs += $(SRC_BOOZ)/booz2_navigation.c
diff --git a/conf/autopilot/subsystems/booz2_ins_hff.makefile b/conf/autopilot/subsystems/booz2_ins_hff.makefile
new file mode 100644
index 0000000000..8c3b1b6073
--- /dev/null
+++ b/conf/autopilot/subsystems/booz2_ins_hff.makefile
@@ -0,0 +1,9 @@
+#
+# simple horizontal filter for INS
+#
+
+ap.CFLAGS += -DUSE_HFF
+ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
+
+sim.CFLAGS += -DUSE_HFF
+sim.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
diff --git a/conf/messages.xml b/conf/messages.xml
index 16fe614c0d..a520dcaa5d 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1069,10 +1069,8 @@
-
-
@@ -1080,10 +1078,8 @@
-
-
diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index fbc246aeb3..b6af40f28c 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -52,9 +52,11 @@ struct LtpDef_i booz_ins_ltp_def;
bool_t booz_ins_ltp_initialised;
struct NedCoor_i booz_ins_gps_pos_cm_ned;
struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
+#ifdef USE_HFF
/* horizontal gps transformed to NED in meters as float */
struct FloatVect2 booz_ins_gps_pos_m_ned;
struct FloatVect2 booz_ins_gps_speed_m_s_ned;
+#endif
/* barometer */
#ifdef USE_VFF
@@ -72,11 +74,6 @@ struct EnuCoor_i booz_ins_enu_pos;
struct EnuCoor_i booz_ins_enu_speed;
struct EnuCoor_i booz_ins_enu_accel;
-#ifdef USE_HFF
-/* counter for hff propagation*/
-uint8_t b2_hff_ps_counter;
-#endif
-
void booz_ins_init() {
#ifdef USE_NAV_INS_INIT
@@ -93,8 +90,7 @@ void booz_ins_init() {
b2_vff_init(0., 0., 0.);
#endif
#ifdef USE_HFF
- b2_hff_init(0., 0., 0., 0., 0., 0.);
- b2_hff_ps_counter = 1;
+ b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(booz_ins_ltp_pos);
INT32_VECT3_ZERO(booz_ins_ltp_speed);
@@ -135,34 +131,18 @@ void booz_ins_propagate() {
#endif /* USE_VFF */
#ifdef USE_HFF
- if (b2_hff_ps_counter == HFF_PRESCALER) {
- b2_hff_ps_counter = 1;
- if (booz_ahrs.status == BOOZ_AHRS_RUNNING ) {
- /* compute float ltp mean acceleration */
- booz_ahrs_compute_accel_mean(HFF_PRESCALER);
- struct Int32Vect3 mean_accel_body;
- INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, booz_ahrs_accel_mean);
- struct Int32Vect3 mean_accel_ltp;
- INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body);
- float x_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
- float y_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
-#ifdef GPS_LAG
- b2_hff_store_accel(x_accel_mean_f, y_accel_mean_f);
-#endif
- if ( booz_ins_ltp_initialised ) {
- /* 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_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);
- }
+ if (booz_ahrs.status == BOOZ_AHRS_RUNNING ) {
+ /* propagate horizontal filter */
+ b2_hff_propagate();
+ if ( booz_ins_ltp_initialised ) {
+ /* update ins state from horizontal filter */
+ 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 {
- b2_hff_ps_counter++;
}
#else
booz_ins_ltp_accel.x = accel_ltp.x;
@@ -208,13 +188,13 @@ void booz_ins_update_gps(void) {
booz_ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
+ ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
+
+#ifdef USE_HFF
VECT2_ASSIGN(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x, booz_ins_gps_pos_cm_ned.y);
VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
- ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x, booz_ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
-
-#ifdef USE_HFF
b2_hff_update_gps();
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);
diff --git a/sw/airborne/booz/booz2_ins.h b/sw/airborne/booz/booz2_ins.h
index 28a71ba913..d1f1f41ab1 100644
--- a/sw/airborne/booz/booz2_ins.h
+++ b/sw/airborne/booz/booz2_ins.h
@@ -50,9 +50,11 @@ extern struct NedCoor_i booz_ins_ltp_accel;
extern struct EnuCoor_i booz_ins_enu_pos;
extern struct EnuCoor_i booz_ins_enu_speed;
extern struct EnuCoor_i booz_ins_enu_accel;
+#ifdef USE_HFF
/* horizontal gps transformed to NED in meters as float */
extern struct FloatVect2 booz_ins_gps_pos_m_ned;
extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
+#endif
extern void booz_ins_init( void );
extern void booz_ins_periodic( void );
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index f404366868..d82007f044 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -454,31 +454,27 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_HFF
#include "ins/booz2_hf_float.h"
-#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \
- DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \
- &b2_hff_x_meas, \
- &b2_hff_state.xdotdot, \
+#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \
+ DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \
+ &b2_hff_x_meas, \
+ &b2_hff_state.xdotdot, \
&b2_hff_state.x, \
- &b2_hff_state.xdot, \
- &b2_hff_state.xbias, \
+ &b2_hff_state.xdot, \
&b2_hff_state.xP[0][0], \
- &b2_hff_state.xP[1][1], \
- &b2_hff_state.xP[2][2]); \
+ &b2_hff_state.xP[1][1]); \
}
-#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \
- DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
- &b2_hff_y_meas, \
- &b2_hff_state.ydotdot, \
+#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \
+ DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
+ &b2_hff_y_meas, \
+ &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]); \
+ &b2_hff_state.ydot, \
+ &b2_hff_state.yP[0][0], \
+ &b2_hff_state.yP[1][1]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
- &b2_hff_save.lag_counter, \
+ &b2_hff_rb_last->lag_counter, \
&lag_counter_err, \
&save_counter); \
}
diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c
index 33970c029d..064aa8b7a7 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.c
+++ b/sw/airborne/booz/ins/booz2_hf_float.c
@@ -24,15 +24,11 @@
#include "booz2_hf_float.h"
#include "booz2_ins.h"
+#include "booz_imu.h"
+#include "booz_ahrs.h"
#include
-/*
-X_x = [ x xdot xbias ]
-X_y = [ y ydot ybias ]
-
-
-*/
/* horizontal filter propagation frequency */
#define HFF_FREQ 512./HFF_PRESCALER
@@ -40,77 +36,27 @@ X_y = [ y ydot ybias ]
/* initial covariance diagonal */
#define INIT_PXX 1.
-#define INIT_PXX_BIAS 0.1
/* process noise (is the same for x and y)*/
#define ACCEL_NOISE 0.5
#define Q ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
#define Qdotdot ACCEL_NOISE*DT_HFILTER
-#define Qbiasbias 1e-7*HFF_PRESCALER
//TODO: proper measurement noise
#define Rpos 5.
#define Rspeed 1.
+/*
+
+ X_x = [ x xdot]
+ X_y = [ y ydot]
+
+
+*/
/* output filter states */
-struct hfilter_f b2_hff_state;
-/* saved state when gps was valid */
-struct hfilter_f b2_hff_save;
+struct HfilterFloat b2_hff_state;
/* pointer to filter states to operate on */
-struct hfilter_f *b2_hff_work;
+//struct HfilterFloat *b2_hff_work;
-/* 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]; */
-
-#ifdef GPS_LAG
-/* GPS_LAG is defined in seconds
- * GPS_LAG_PS in multiple of prescaler
- */
-/* number of propagaton steps to redo according to GPS_LAG */
-#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]; */
-
-#define BUF_MAXN GPS_LAG_N+2
-/* buffer with past mean accel values for redoing the propagation */
-struct FloatVect2 past_accel[BUF_MAXN];
-
-/* variables for accel buffer */
-uint8_t buf_r; /* pos to read from, oldest measurement */
-uint8_t buf_w; /* pos to write to */
-uint8_t buf_n; /* number of elements in buffer */
-
-/* number of propagation steps since state was saved */
-/* 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;
-
-/* counts down the propagation steps until the filter state is saved again */
-int8_t save_counter;
-
-static inline void b2_hff_get_past_accel(int back_n);
-static inline void b2_hff_rollback_filter(void);
-static inline void b2_hff_save_filter(void);
-#endif /* GPS_LAG */
/* acceleration used in propagation step */
float b2_hff_xaccel;
@@ -120,284 +66,383 @@ float b2_hff_yaccel;
float b2_hff_x_meas;
float b2_hff_y_meas;
-
-static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias);
-static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias);
-
-static inline void b2_hff_propagate_x(void);
-static inline void b2_hff_propagate_y(void);
-
-static inline void b2_hff_update_x(float x_meas);
-static inline void b2_hff_update_y(float y_meas);
-
-static inline void b2_hff_update_xdot(float v);
-static inline void b2_hff_update_ydot(float v);
+/* counter for hff propagation*/
+int b2_hff_ps_counter;
+/*
+ * For GPS lag compensation
+ *
+ *
+ *
+ */
-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;
- b2_hff_save.lag_counter = 0;
- b2_hff_state.lag_counter = GPS_LAG_N;
- lag_counter_err = 0;
+/*
+ * GPS_LAG is defined in seconds in airframe file
+ */
+
+/* number of propagaton steps to redo according to GPS_LAG */
+#define GPS_LAG_N ((int) (GPS_LAG * HFF_FREQ + 0.5))
+/* number of propagation steps between two GPS updates */
+#define GPS_DT_N ((int) (HFF_FREQ / 4))
+
+/* maximum number of past propagation steps to rerun per ap cycle
+ * make sure GPS_LAG_N/MAX_PP_STEPS < GPS_DT_N
+ */
+#define MAX_PP_STEPS 6
+
+/* variables for accel buffer */
+#define ACC_BUF_MAXN GPS_LAG_N+10
+struct FloatVect2 past_accel[ACC_BUF_MAXN]; /* buffer with past mean accel values for redoing the propagation */
+uint8_t acc_buf_r; /* pos to read from, oldest measurement */
+uint8_t acc_buf_w; /* pos to write to */
+uint8_t acc_buf_n; /* number of elements in buffer */
+
+
+/*
+ * stuff for ringbuffer to store past filter states
+ */
+#define HFF_RB_MAXN ((int) (GPS_LAG * 4))
+#define INC_RB_POINTER(ptr) { \
+ if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \
+ ptr = b2_hff_rb; \
+ else \
+ ptr++; \
+ }
+
+struct HfilterFloat b2_hff_rb[HFF_RB_MAXN]; /* ringbuffer with state and covariance when GPS was valid */
+struct HfilterFloat *b2_hff_rb_put; /* write pointer */
+#endif /* GPS_LAG */
+
+struct HfilterFloat *b2_hff_rb_last; /* read pointer */
+int b2_hff_rb_n; /* fill count */
+
+
+/* by how many steps the estimated GPS validity point in time differed from GPS_LAG_N */
+int8_t lag_counter_err;
+
+/* counts down the propagation steps until the filter state is saved again */
+int8_t save_counter;
+int8_t past_save_counter;
+
+#ifdef GPS_LAG
+static inline void b2_hff_get_past_accel(int back_n);
+static inline void b2_hff_rb_put_state(struct HfilterFloat* source);
+static inline void b2_hff_rb_next(void);
+static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source);
#endif
+
+
+
+static inline void b2_hff_init_x(float init_x, float init_xdot);
+static inline void b2_hff_init_y(float init_y, float init_ydot);
+
+static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work);
+static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work);
+
+static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas);
+static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas);
+
+static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float v);
+static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float v);
+
+
+
+void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
+ b2_hff_init_x(init_x, init_xdot);
+ b2_hff_init_y(init_y, init_ydot);
+#ifdef GPS_LAG
+ acc_buf_r = 0;
+ acc_buf_w = 0;
+ acc_buf_n = 0;
+ b2_hff_rb_put = b2_hff_rb;
+ b2_hff_rb_last = b2_hff_rb;
+ b2_hff_state.lag_counter = GPS_LAG_N;
+#else
+ b2_hff_rb_last = NULL;
+ b2_hff_state.lag_counter = 0;
+#endif
+ b2_hff_rb_n = 0;
+ b2_hff_state.rollback = 0;
+ lag_counter_err = 0;
+ b2_hff_ps_counter = 1;
}
-static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias) {
+static inline void b2_hff_init_x(float init_x, float init_xdot) {
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 buf_n) {
- //printf("Cannot go back %d steps, going back only %d instead!\n", back_n, buf_n);
- back_n = buf_n;
+ if (back_n > acc_buf_n) {
+ //printf("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n);
+ back_n = acc_buf_n;
+ } else if (back_n <= 0) {
+ //printf("Cannot go back %d steps, not getting past accel value!\n", back_n);
+ return;
}
- //i = (buf_r + n) < BUF_MAXN ? (buf_r + n) : (buf_r + n - BUF_MAXN);
- i = (buf_w-1 - back_n) > 0 ? (buf_w-1 - back_n) : (buf_w-1 + BUF_MAXN - back_n);
+ i = (acc_buf_w - back_n) > 0 ? (acc_buf_w - back_n) : (acc_buf_w + ACC_BUF_MAXN - back_n);
b2_hff_xaccel = past_accel[i].x;
b2_hff_yaccel = past_accel[i].y;
}
-/* 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; */
-/* 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;
+static inline void b2_hff_rb_put_state(struct HfilterFloat* source) {
+ /* copy state from source into buffer */
+ b2_hff_set_state(b2_hff_rb_put, source);
+ b2_hff_rb_put->lag_counter = 0;
+ b2_hff_rb_put->rollback = 0;
+
+ /* forward write pointer */
+ INC_RB_POINTER(b2_hff_rb_put);
+
+ /* increase fill count and forward last pointer if neccessary */
+ if (b2_hff_rb_n < HFF_RB_MAXN) {
+ b2_hff_rb_n++;
+ } else {
+ INC_RB_POINTER(b2_hff_rb_last);
+ }
}
-/* 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;
+static inline void b2_hff_rb_next(void) {
+ if (b2_hff_rb_n > 0) {
+ INC_RB_POINTER(b2_hff_rb_last);
+ b2_hff_rb_n--;
+ } else {
+ //printf("hff ringbuffer empty!\n");
+ }
+}
+
+
+/* copy source state to dest state */
+static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source) {
+ dest->x = source->x;
+ dest->xdot = source->xdot;
+ dest->xdotdot = source->xdotdot;
+ dest->y = source->y;
+ dest->ydot = source->ydot;
+ dest->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_save.xP[i][j] = b2_hff_state.xP[i][j];
- b2_hff_save.yP[i][j] = b2_hff_state.yP[i][j];
+ dest->xP[i][j] = source->xP[i][j];
+ dest->yP[i][j] = source->yP[i][j];
}
}
- b2_hff_save.lag_counter = 0;
}
-/* 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_state.xP[i][j] = source->xP[i][j];
- b2_hff_state.yP[i][j] = source->yP[i][j];
+static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
+ /* run max MAX_PP_STEPS propagation steps */
+ for (int i=0; i < MAX_PP_STEPS; i++) {
+ b2_hff_get_past_accel(hff_past->lag_counter);
+ b2_hff_propagate_x(hff_past);
+ b2_hff_propagate_y(hff_past);
+ hff_past->lag_counter--;
+
+ if (past_save_counter == 0) {
+ /* next GPS measurement valid at this state -> save */
+ b2_hff_rb_put_state(hff_past);
+ past_save_counter = -1;
+ } else if (save_counter > 0) {
+ past_save_counter--;
+ }
+
+ if (hff_past->lag_counter == 0) {
+ b2_hff_set_state(&b2_hff_state, hff_past);
+ b2_hff_rb_next();
+ break;
}
}
}
#endif
-/*
- 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 b2_hff_propagate(float xaccel, float yaccel) {
+void b2_hff_propagate(void) {
#ifdef GPS_LAG
- /* save filter if now is the estimated GPS validity point in time */
- if (save_counter == 0) {
- b2_hff_save_filter();
- save_counter = -1;
- } else if (save_counter > 0) {
- save_counter--;
+ /* continue to redo the propagation to catch up with the present */
+ if (b2_hff_rb_last->rollback) {
+ b2_hff_propagate_past(b2_hff_rb_last);
}
#endif
- b2_hff_xaccel = xaccel;
- b2_hff_yaccel = yaccel;
- b2_hff_propagate_x();
- b2_hff_propagate_y();
+ /* propagate current state if it is time */
+ if (b2_hff_ps_counter == HFF_PRESCALER) {
+ b2_hff_ps_counter = 1;
+ /* compute float ltp mean acceleration */
+ booz_ahrs_compute_accel_mean(HFF_PRESCALER);
+ struct Int32Vect3 mean_accel_body;
+ INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, booz_ahrs_accel_mean);
+ struct Int32Vect3 mean_accel_ltp;
+ INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body);
+ b2_hff_xaccel = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
+ b2_hff_yaccel = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
+
+ /*
+ * propagate current state
+ */
+ if ( booz_ins_ltp_initialised ) {
+ b2_hff_propagate_x(&b2_hff_state);
+ b2_hff_propagate_y(&b2_hff_state);
+ }
#ifdef GPS_LAG
- if (b2_hff_save.lag_counter < 2*GPS_LAG_N) // prevent from overflowing if no gps available
- b2_hff_save.lag_counter++;
+ b2_hff_store_accel(b2_hff_xaccel, b2_hff_yaccel);
+ /* increase all lag counters */
+ for (int i=0; i < b2_hff_rb_n; i++ ) {
+ (b2_hff_rb_last+i)->lag_counter++;
+ }
+
+ /* save filter state if ringbuffer is empty */
+ if (save_counter == 0) {
+ b2_hff_rb_put_state(&b2_hff_state);
+ save_counter = -1;
+ } else if (save_counter > 0) {
+ save_counter--;
+ }
#endif
+
+ } else {
+ b2_hff_ps_counter++;
+ }
}
-static inline void b2_hff_propagate_x(void) {
- /* update state */
- 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_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_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_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_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_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 = 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();
- }
-#endif
-#ifdef B2_HFF_UPDATE_POS
- b2_hff_update_x(booz_ins_gps_pos_m_ned.x);
- b2_hff_update_y(booz_ins_gps_pos_m_ned.y);
-#endif
+ if (b2_hff_rb_n > 0) {
+ /* roll back if state was saved approx when GPS was valid */
+ lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
+ if (abs(lag_counter_err) < 3) {
+ b2_hff_rb_last->rollback = 1;
+ b2_hff_update_x(b2_hff_rb_last, booz_ins_gps_pos_m_ned.x);
+ b2_hff_update_y(b2_hff_rb_last, booz_ins_gps_pos_m_ned.y);
#ifdef B2_HFF_UPDATE_SPEED
- b2_hff_update_xdot(booz_ins_gps_speed_m_s_ned.x);
- b2_hff_update_ydot(booz_ins_gps_speed_m_s_ned.y);
+ b2_hff_update_xdot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.x);
+ b2_hff_update_ydot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.y);
#endif
-
-#ifdef GPS_LAG
- /* 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=b2_hff_save.lag_counter-1; i >= 0; i--) {
- b2_hff_get_past_accel(i);
- b2_hff_propagate_x();
- b2_hff_propagate_y();
+ past_save_counter = GPS_DT_N + lag_counter_err;
+ b2_hff_propagate_past(b2_hff_rb_last);
+ } else if (lag_counter_err >= GPS_DT_N-2) {
+ /* apparently missed a GPS update, try next saved state */
+ b2_hff_rb_next();
+ b2_hff_update_gps();
}
- b2_hff_set_state(b2_hff_work);
- b2_hff_work = &b2_hff_state;
+ } else {
+ /* ringbuffer empty -> save output filter state at next GPS validity point in time */
+ save_counter = GPS_DT_N - (GPS_LAG_N % GPS_DT_N);
}
- /* reset save counter */
- save_counter = (int)(HFF_FREQ / 4) % GPS_LAG_N;
+#else /* GPS_LAG */
+
+ b2_hff_update_x(&b2_hff_state, booz_ins_gps_pos_m_ned.x);
+ b2_hff_update_y(&b2_hff_state, booz_ins_gps_pos_m_ned.y);
+#ifdef B2_HFF_UPDATE_SPEED
+ b2_hff_update_xdot(&b2_hff_state, booz_ins_gps_speed_m_s_ned.x);
+ b2_hff_update_ydot(&b2_hff_state, booz_ins_gps_speed_m_s_ned.y);
#endif
+
+#endif/* GPS_LAG */
+}
+
+
+
+/*
+ *
+ * Propagation
+ *
+ *
+
+ F = [ 1 dt
+ 0 1 ];
+
+ B = [ dt^2/2 dt]';
+
+ Q = [ 0.01 0
+ 0 0.01];
+
+ Xk1 = F * Xk0 + B * accel;
+
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) {
+ /* update state */
+ hff_work->xdotdot = b2_hff_xaccel;
+ hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot;
+ hff_work->xdot = hff_work->xdot + DT_HFILTER * hff_work->xdotdot;
+ /* update covariance */
+ const float FPF00 = hff_work->xP[0][0] + DT_HFILTER * ( hff_work->xP[1][0] + hff_work->xP[0][1] + DT_HFILTER * hff_work->xP[1][1] );
+ const float FPF01 = hff_work->xP[0][1] + DT_HFILTER * hff_work->xP[1][1];
+ const float FPF10 = hff_work->xP[1][0] + DT_HFILTER * hff_work->xP[1][1];
+ const float FPF11 = hff_work->xP[1][1];
+
+ hff_work->xP[0][0] = FPF00 + Q;
+ hff_work->xP[0][1] = FPF01;
+ hff_work->xP[1][0] = FPF10;
+ hff_work->xP[1][1] = FPF11 + Qdotdot;
+}
+
+static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work) {
+ /* update state */
+ hff_work->ydotdot = b2_hff_yaccel;
+ hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot;
+ hff_work->ydot = hff_work->ydot + DT_HFILTER * hff_work->ydotdot;
+ /* update covariance */
+ const float FPF00 = hff_work->yP[0][0] + DT_HFILTER * ( hff_work->yP[1][0] + hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1] );
+ const float FPF01 = hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1];
+ const float FPF10 = hff_work->yP[1][0] + DT_HFILTER * hff_work->yP[1][1];
+ const float FPF11 = hff_work->yP[1][1];
+
+ hff_work->yP[0][0] = FPF00 + Q;
+ hff_work->yP[0][1] = FPF01;
+ hff_work->yP[1][0] = FPF10;
+ hff_work->yP[1][1] = FPF11 + Qdotdot;
}
/*
- H = [1 0 0];
+ *
+ * Update position
+ *
+ *
+
+ H = [1 0];
R = 0.1;
// state residual
y = pos_measurement - H * Xm;
@@ -411,85 +456,64 @@ void b2_hff_update_gps(void) {
Pp = Pm - K*H*Pm;
*/
void b2_hff_update_pos (float posx, float posy) {
- b2_hff_update_x(posx);
- b2_hff_update_y(posy);
+ b2_hff_update_x(&b2_hff_state, posx);
+ b2_hff_update_y(&b2_hff_state, posy);
}
-static inline void b2_hff_update_x(float x_meas) {
+static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas) {
b2_hff_x_meas = x_meas;
- 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;
+ const float y = x_meas - hff_work->x;
+ const float S = hff_work->xP[0][0] + Rpos;
+ const float K1 = hff_work->xP[0][0] * 1/S;
+ const float K2 = hff_work->xP[1][0] * 1/S;
- 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;
+ hff_work->x = hff_work->x + K1 * y;
+ hff_work->xdot = hff_work->xdot + K2 * y;
- 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_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;
+ const float P11 = (1. - K1) * hff_work->xP[0][0];
+ const float P12 = (1. - K1) * hff_work->xP[0][1];
+ const float P21 = -K2 * hff_work->xP[0][0] + hff_work->xP[1][0];
+ const float P22 = -K2 * hff_work->xP[0][1] + hff_work->xP[1][1];
+ hff_work->xP[0][0] = P11;
+ hff_work->xP[0][1] = P12;
+ hff_work->xP[1][0] = P21;
+ hff_work->xP[1][1] = P22;
}
-static inline void b2_hff_update_y(float y_meas) {
+static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas) {
b2_hff_y_meas = y_meas;
- 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;
+ const float y = y_meas - hff_work->y;
+ const float S = hff_work->yP[0][0] + Rpos;
+ const float K1 = hff_work->yP[0][0] * 1/S;
+ const float K2 = hff_work->yP[1][0] * 1/S;
- 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;
+ hff_work->y = hff_work->y + K1 * y;
+ hff_work->ydot = hff_work->ydot + K2 * y;
- 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_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;
+ const float P11 = (1. - K1) * hff_work->yP[0][0];
+ const float P12 = (1. - K1) * hff_work->yP[0][1];
+ const float P21 = -K2 * hff_work->yP[0][0] + hff_work->yP[1][0];
+ const float P22 = -K2 * hff_work->yP[0][1] + hff_work->yP[1][1];
+ hff_work->yP[0][0] = P11;
+ hff_work->yP[0][1] = P12;
+ hff_work->yP[1][0] = P21;
+ hff_work->yP[1][1] = P22;
}
/*
- H = [0 1 0];
+ *
+ * Update speed
+ *
+ *
+
+ H = [0 1];
R = 0.1;
// state residual
yd = vx - H * Xm;
@@ -503,74 +527,48 @@ static inline void b2_hff_update_y(float y_meas) {
Pp = Pm - K*H*Pm;
*/
void b2_hff_update_v(float xspeed, float yspeed) {
- b2_hff_update_xdot(xspeed);
- b2_hff_update_ydot(yspeed);
+ b2_hff_update_xdot(&b2_hff_state, xspeed);
+ b2_hff_update_ydot(&b2_hff_state, yspeed);
}
-static inline void b2_hff_update_xdot(float v) {
- 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;
+static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float v) {
+ const float yd = v - hff_work->xdot;
+ const float S = hff_work->xP[1][1] + Rspeed;
+ const float K1 = hff_work->xP[0][1] * 1/S;
+ const float K2 = hff_work->xP[1][1] * 1/S;
- 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;
+ hff_work->x = hff_work->x + K1 * yd;
+ hff_work->xdot = hff_work->xdot + K2 * yd;
- 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_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;
+ const float P11 = -K1 * hff_work->xP[1][0] + hff_work->xP[0][0];
+ const float P12 = -K1 * hff_work->xP[1][1] + hff_work->xP[0][1];
+ const float P21 = (1. - K2) * hff_work->xP[1][0];
+ const float P22 = (1. - K2) * hff_work->xP[1][1];
+ hff_work->xP[0][0] = P11;
+ hff_work->xP[0][1] = P12;
+ hff_work->xP[1][0] = P21;
+ hff_work->xP[1][1] = P22;
}
-static inline void b2_hff_update_ydot(float v) {
- 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;
+static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float v) {
+ const float yd = v - hff_work->ydot;
+ const float S = hff_work->yP[1][1] + Rspeed;
+ const float K1 = hff_work->yP[0][1] * 1/S;
+ const float K2 = hff_work->yP[1][1] * 1/S;
- 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;
+ hff_work->y = hff_work->y + K1 * yd;
+ hff_work->ydot = hff_work->ydot + K2 * yd;
- 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_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;
+ const float P11 = -K1 * hff_work->yP[1][0] + hff_work->yP[0][0];
+ const float P12 = -K1 * hff_work->yP[1][1] + hff_work->yP[0][1];
+ const float P21 = (1. - K2) * hff_work->yP[1][0];
+ const float P22 = (1. - K2) * hff_work->yP[1][1];
+ hff_work->yP[0][0] = P11;
+ hff_work->yP[0][1] = P12;
+ hff_work->yP[1][0] = P21;
+ hff_work->yP[1][1] = P22;
}
diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/booz/ins/booz2_hf_float.h
index 659f2de01e..7fa91b41bd 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.h
+++ b/sw/airborne/booz/ins/booz2_hf_float.h
@@ -33,9 +33,8 @@
#endif
#define B2_HFF_UPDATE_SPEED
-#define B2_HFF_UPDATE_POS
-struct hfilter_f {
+struct HfilterFloat {
float x;
float xbias;
float xdot;
@@ -47,40 +46,26 @@ struct hfilter_f {
float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
uint8_t lag_counter;
+ uint8_t rollback;
};
-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_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 struct HfilterFloat b2_hff_state;
extern float b2_hff_x_meas;
extern float b2_hff_y_meas;
-extern void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias);
-extern void b2_hff_propagate(float xaccel, float yaccel);
+extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
+extern void b2_hff_propagate(void);
extern void b2_hff_update_gps(void);
extern void b2_hff_update_pos(float xpos, float ypos);
extern void b2_hff_update_v(float xspeed, float yspeed);
#ifdef GPS_LAG
extern void b2_hff_store_accel(float x, float y);
-/* extern uint8_t lag_counter; */
+#endif
+extern struct HfilterFloat *b2_hff_rb_last;
extern int8_t lag_counter_err;
extern int8_t save_counter;
-#endif
#endif /* BOOZ2_HF_FLOAT_H */