diff --git a/conf/abi.xml b/conf/abi.xml
index e57d6d9e8f..ef726c3548 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -97,6 +97,14 @@
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow.xml b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow.xml
index 01eaf87ef0..ed90b63448 100644
--- a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow.xml
@@ -23,10 +23,8 @@
-
-
-
-
+
+
@@ -43,9 +41,7 @@
-
-
-
+
diff --git a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml
index da98cf1f98..c74271706a 100644
--- a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml
+++ b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml
@@ -11,7 +11,7 @@
-
+
@@ -24,9 +24,7 @@
-
-
-
+
@@ -43,9 +41,7 @@
-
-
-
+
diff --git a/conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml b/conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml
index f55509532d..4f42f24608 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml
@@ -18,14 +18,11 @@
-
+
-
-
-
@@ -35,10 +32,9 @@
-
-
+
@@ -62,8 +58,7 @@
-->
-
-
+
diff --git a/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml b/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
index 3e4508dd27..b19ad36234 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
@@ -12,7 +12,7 @@
-
+
@@ -24,10 +24,8 @@
-
-
-
-
+
+
@@ -55,9 +53,7 @@
-
-
-
+
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index 1fd8955a63..f4c598806d 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -292,7 +292,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
- settings=""
+ settings="settings/rotorcraft_basic.xml"
settings_modules="modules/cv_opticflow.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue"
/>
diff --git a/conf/modules/ins_hff_extended.xml b/conf/modules/ins_hff_extended.xml
new file mode 100644
index 0000000000..b79db4157b
--- /dev/null
+++ b/conf/modules/ins_hff_extended.xml
@@ -0,0 +1,43 @@
+
+
+
+
+
+ extended INS with vertical filter using sonar.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c
index 5d6c4350d3..096688be5a 100644
--- a/sw/airborne/subsystems/ins/hf_float.c
+++ b/sw/airborne/subsystems/ins/hf_float.c
@@ -56,13 +56,15 @@
#define HFF_PRESCALER 16
#elif AHRS_PROPAGATE_FREQUENCY == 500
#define HFF_PRESCALER 10
+#elif AHRS_PROPAGATE_FREQUENCY == 200
+#define HFF_PRESCALER 6
#else
#error "HFF_PRESCALER not set, needs to be a divisor of AHRS_PROPAGATE_FREQUENCY"
#endif
#endif
/** horizontal filter propagation frequency */
-#define HFF_FREQ ((AHRS_PROPAGATE_FREQUENCY)/HFF_PRESCALER)
+#define HFF_FREQ (AHRS_PROPAGATE_FREQUENCY / HFF_PRESCALER)
#define DT_HFILTER (1./HFF_FREQ)
/** initial covariance diagonal */
@@ -71,8 +73,8 @@
#ifndef HFF_ACCEL_NOISE
#define HFF_ACCEL_NOISE 0.5
#endif
-#define Q HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
-#define Qdotdot HFF_ACCEL_NOISE*DT_HFILTER
+#define Q HFF_ACCEL_NOISE
+#define Qdotdot HFF_ACCEL_NOISE
//TODO: proper measurement noise
#ifndef HFF_R_POS
@@ -82,15 +84,15 @@
#define HFF_R_POS_MIN 3.
#endif
-#ifndef HFF_R_SPEED
-#define HFF_R_SPEED 2.
+#ifndef HFF_R_GPS_SPEED
+#define HFF_R_GPS_SPEED 2.
#endif
-#ifndef HFF_R_SPEED_MIN
-#define HFF_R_SPEED_MIN 1.
+#ifndef HFF_R_GPS_SPEED_MIN
+#define HFF_R_GPS_SPEED_MIN 0.25
#endif
-#ifndef HFF_UPDATE_SPEED
-#define HFF_UPDATE_SPEED TRUE
+#ifndef HFF_UPDATE_GPS_SPEED
+#define HFF_UPDATE_GPS_SPEED TRUE
#endif
#ifndef HFF_LOWPASS_CUTOFF_FREQUENCY
@@ -110,36 +112,34 @@ Butterworth2LowPass_int filter_z;
float Rgps_pos, Rgps_vel;
/*
-
- X_x = [ x xdot]
- X_y = [ y ydot]
-
-
+ X_x = [ x xdot xbias ]
+ X_y = [ y ydot ybias ]
*/
-/* output filter states */
-struct HfilterFloat b2_hff_state;
+/* output filter states */
+struct HfilterFloat hff;
/* last acceleration measurement */
-static float b2_hff_xdd_meas;
-static float b2_hff_ydd_meas;
+static float hff_xdd_meas = 0;
+static float hff_ydd_meas = 0;
/* last velocity measurement */
-static float b2_hff_xd_meas;
-static float b2_hff_yd_meas;
+static float hff_xd_meas = 0;
+static float hff_yd_meas = 0;
/* last position measurement */
-static float b2_hff_x_meas;
-static float b2_hff_y_meas;
+static float hff_x_meas = 0;
+static float hff_y_meas = 0;
/** counter for hff propagation*/
-static int b2_hff_ps_counter;
+static int hff_ps_counter;
+
+/* default parameters */
+#define Qbiasbias 1e-7
/*
* For GPS lag compensation
*
- *
- *
*/
#ifdef GPS_LAG
/*
@@ -177,19 +177,19 @@ static unsigned int acc_buf_n; ///< number of elements in buffer
*/
#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; \
+ if (ptr == &hff_rb[HFF_RB_MAXN-1]) \
+ ptr = hff_rb; \
else \
ptr++; \
}
/** ringbuffer with state and covariance when GPS was valid */
-struct HfilterFloat b2_hff_rb[HFF_RB_MAXN];
-struct HfilterFloat *b2_hff_rb_put; ///< ringbuffer write pointer
+struct HfilterFloat hff_rb[HFF_RB_MAXN];
+struct HfilterFloat *hff_rb_put; ///< ringbuffer write pointer
#endif /* GPS_LAG */
-struct HfilterFloat *b2_hff_rb_last; ///< ringbuffer read pointer
-static int b2_hff_rb_n; ///< ringbuffer fill count
+struct HfilterFloat *hff_rb_last; ///< ringbuffer read pointer
+static int hff_rb_n; ///< ringbuffer fill count
/** by how many steps the estimated GPS validity point in time differed from GPS_LAG_N */
@@ -203,28 +203,27 @@ static int past_save_counter;
#define SAVE_DONE -2
#define HFF_LOST_LIMIT 1000
-static uint16_t b2_hff_lost_limit;
-static uint16_t b2_hff_lost_counter;
+static uint16_t hff_lost_limit;
+static uint16_t hff_lost_counter, hff_speed_lost_counter;
#ifdef GPS_LAG
-static void b2_hff_get_past_accel(unsigned int back_n);
-static void b2_hff_rb_put_state(struct HfilterFloat *source);
-static void b2_hff_rb_drop_last(void);
-static void b2_hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source);
+static void hff_get_past_accel(unsigned int back_n);
+static void hff_rb_put_state(struct HfilterFloat *source);
+static void hff_rb_drop_last(void);
+static void hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source);
#endif
+static void hff_init_x(float init_x, float init_xdot, float init_xbias);
+static void hff_init_y(float init_y, float init_ydot, float init_ybias);
-static void b2_hff_init_x(float init_x, float init_xdot);
-static void b2_hff_init_y(float init_y, float init_ydot);
+static void hff_propagate_x(struct HfilterFloat *filt, float dt);
+static void hff_propagate_y(struct HfilterFloat *filt, float dt);
-static void b2_hff_propagate_x(struct HfilterFloat *hff_work, float dt);
-static void b2_hff_propagate_y(struct HfilterFloat *hff_work, float dt);
+static void hff_update_x(struct HfilterFloat *filt, float x_meas, float Rpos);
+static void hff_update_y(struct HfilterFloat *filt, float y_meas, float Rpos);
-static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos);
-static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos);
-
-static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel);
-static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel);
+static void hff_update_xdot(struct HfilterFloat *filt, float vel, float Rvel);
+static void hff_update_ydot(struct HfilterFloat *filt, float vel, float Rvel);
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -232,32 +231,36 @@ static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float R
static void send_hff(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_HFF(trans, dev, AC_ID,
- &b2_hff_state.x,
- &b2_hff_state.y,
- &b2_hff_state.xdot,
- &b2_hff_state.ydot,
- &b2_hff_state.xdotdot,
- &b2_hff_state.ydotdot);
+ &hff.x,
+ &hff.y,
+ &hff.xdot,
+ &hff.ydot,
+ &hff.xdotdot,
+ &hff.ydotdot,
+ &hff.xbias,
+ &hff.ybias);
}
static void send_hff_debug(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_HFF_DBG(trans, dev, AC_ID,
- &b2_hff_x_meas,
- &b2_hff_y_meas,
- &b2_hff_xd_meas,
- &b2_hff_yd_meas,
- &b2_hff_state.xP[0][0],
- &b2_hff_state.yP[0][0],
- &b2_hff_state.xP[1][1],
- &b2_hff_state.yP[1][1]);
+ &hff_x_meas,
+ &hff_y_meas,
+ &hff_xd_meas,
+ &hff_yd_meas,
+ &hff.xP[0][0],
+ &hff.yP[0][0],
+ &hff.xP[1][1],
+ &hff.yP[1][1],
+ &hff.xP[2][2],
+ &hff.yP[2][2]);
}
#ifdef GPS_LAG
static void send_hff_gps(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_HFF_GPS(trans, dev, AC_ID,
- &(b2_hff_rb_last->lag_counter),
+ &(hff_rb_last->lag_counter),
&lag_counter_err,
&save_counter);
}
@@ -265,22 +268,22 @@ static void send_hff_gps(struct transport_tx *trans, struct link_device *dev)
#endif
-void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
+void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
{
Rgps_pos = HFF_R_POS;
- Rgps_vel = HFF_R_SPEED;
- b2_hff_init_x(init_x, init_xdot);
- b2_hff_init_y(init_y, init_ydot);
+ Rgps_vel = HFF_R_GPS_SPEED;
+ hff_init_x(init_x, init_xdot, 0.f);
+ hff_init_y(init_y, init_ydot, 0.f);
#ifdef GPS_LAG
/* init buffer for past mean accel values */
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_rb_last->rollback = false;
- b2_hff_rb_last->lag_counter = 0;
- b2_hff_state.lag_counter = GPS_LAG_N;
+ hff_rb_put = hff_rb;
+ hff_rb_last = hff_rb;
+ hff_rb_last->rollback = false;
+ hff_rb_last->lag_counter = 0;
+ hff.lag_counter = GPS_LAG_N;
#ifdef SITL
printf("GPS_LAG: %f\n", GPS_LAG);
printf("GPS_LAG_N: %d\n", GPS_LAG_N);
@@ -289,17 +292,18 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
#endif
#else
- b2_hff_rb_last = &b2_hff_state;
- b2_hff_state.lag_counter = 0;
+ hff_rb_last = &hff;
+ hff.lag_counter = 0;
#endif
- b2_hff_rb_n = 0;
- b2_hff_state.rollback = false;
+ hff_rb_n = 0;
+ hff.rollback = false;
lag_counter_err = 0;
save_counter = -1;
past_save_counter = SAVE_DONE;
- b2_hff_ps_counter = 1;
- b2_hff_lost_counter = 0;
- b2_hff_lost_limit = HFF_LOST_LIMIT;
+ hff_lost_counter = 0;
+ hff_speed_lost_counter = 0;
+ hff_lost_limit = HFF_LOST_LIMIT;
+ hff_ps_counter = 0;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF, send_hff);
@@ -314,34 +318,36 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
init_butterworth_2_low_pass_int(&filter_z, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0);
}
-static void b2_hff_init_x(float init_x, float init_xdot)
+static void hff_init_x(float init_x, float init_xdot, float init_xbias)
{
- b2_hff_state.x = init_x;
- b2_hff_state.xdot = init_xdot;
+ hff.x = init_x;
+ hff.xdot = init_xdot;
+ hff.xbias = init_xbias;
int i, j;
for (i = 0; i < HFF_STATE_SIZE; i++) {
for (j = 0; j < HFF_STATE_SIZE; j++) {
- b2_hff_state.xP[i][j] = 0.;
+ hff.xP[i][j] = 0.;
}
- b2_hff_state.xP[i][i] = INIT_PXX;
+ hff.xP[i][i] = INIT_PXX;
}
}
-static void b2_hff_init_y(float init_y, float init_ydot)
+static void hff_init_y(float init_y, float init_ydot, float init_ybias)
{
- b2_hff_state.y = init_y;
- b2_hff_state.ydot = init_ydot;
+ hff.y = init_y;
+ hff.ydot = init_ydot;
+ hff.ybias = init_ybias;
int i, j;
for (i = 0; i < HFF_STATE_SIZE; i++) {
for (j = 0; j < HFF_STATE_SIZE; j++) {
- b2_hff_state.yP[i][j] = 0.;
+ hff.yP[i][j] = 0.;
}
- b2_hff_state.yP[i][i] = INIT_PXX;
+ hff.yP[i][i] = INIT_PXX;
}
}
#ifdef GPS_LAG
-static void b2_hff_store_accel_ltp(float x, float y)
+static void hff_store_accel_ltp(float x, float y)
{
past_accel[acc_buf_w].x = x;
past_accel[acc_buf_w].y = y;
@@ -355,7 +361,7 @@ static void b2_hff_store_accel_ltp(float x, float y)
}
/* get the accel values from back_n steps ago */
-static void b2_hff_get_past_accel(unsigned int back_n)
+static void hff_get_past_accel(unsigned int back_n)
{
int i;
if (back_n > acc_buf_n) {
@@ -370,46 +376,46 @@ static void b2_hff_get_past_accel(unsigned int back_n)
} else {
i = acc_buf_w - back_n;
}
- b2_hff_xdd_meas = past_accel[i].x;
- b2_hff_ydd_meas = past_accel[i].y;
+ hff_xdd_meas = past_accel[i].x;
+ hff_ydd_meas = past_accel[i].y;
PRINT_DBG(3, ("get past accel. buf_n: %2d \tbuf_w: %2d \tback_n: %2d \ti: %2d \txdd: %f \tydd: %f\n", acc_buf_n,
- acc_buf_w, back_n, i, b2_hff_xdd_meas, b2_hff_ydd_meas));
+ acc_buf_w, back_n, i, hff_xdd_meas, hff_ydd_meas));
}
-static void b2_hff_rb_put_state(struct HfilterFloat *source)
+static void 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 = false;
+ hff_set_state(hff_rb_put, source);
+ hff_rb_put->lag_counter = 0;
+ hff_rb_put->rollback = false;
/* forward write pointer */
- INC_RB_POINTER(b2_hff_rb_put);
+ INC_RB_POINTER(hff_rb_put);
/* increase fill count and forward last pointer if neccessary */
- if (b2_hff_rb_n < HFF_RB_MAXN) {
- b2_hff_rb_n++;
+ if (hff_rb_n < HFF_RB_MAXN) {
+ hff_rb_n++;
} else {
- INC_RB_POINTER(b2_hff_rb_last);
+ INC_RB_POINTER(hff_rb_last);
}
- PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n));
+ PRINT_DBG(2, ("put state. fill count now: %d\n", hff_rb_n));
}
-static void b2_hff_rb_drop_last(void)
+static void hff_rb_drop_last(void)
{
- if (b2_hff_rb_n > 0) {
- INC_RB_POINTER(b2_hff_rb_last);
- b2_hff_rb_n--;
+ if (hff_rb_n > 0) {
+ INC_RB_POINTER(hff_rb_last);
+ hff_rb_n--;
} else {
PRINT_DBG(2, ("hff ringbuffer empty!\n"));
- b2_hff_rb_last->lag_counter = 0;
- b2_hff_rb_last->rollback = false;
+ hff_rb_last->lag_counter = 0;
+ hff_rb_last->rollback = false;
}
- PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n));
+ PRINT_DBG(2, ("drop last state. fill count now: %d\n", hff_rb_n));
}
/* copy source state to dest state */
-static void b2_hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source)
+static void hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source)
{
dest->x = source->x;
dest->xdot = source->xdot;
@@ -425,16 +431,16 @@ static void b2_hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *sou
}
}
-static void b2_hff_propagate_past(struct HfilterFloat *hff_past)
+static void hff_propagate_past(struct HfilterFloat *filt_past)
{
PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter));
/* run max MAX_PP_STEPS propagation steps */
for (int i = 0; i < MAX_PP_STEPS; i++) {
if (hff_past->lag_counter > 0) {
- b2_hff_get_past_accel(hff_past->lag_counter);
+ hff_get_past_accel(hff_past->lag_counter);
PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
- b2_hff_propagate_x(hff_past, DT_HFILTER);
- b2_hff_propagate_y(hff_past, DT_HFILTER);
+ hff_propagate_x(hff_past, DT_HFILTER);
+ hff_propagate_y(hff_past, DT_HFILTER);
hff_past->lag_counter--;
if (past_save_counter > 0) {
@@ -443,12 +449,12 @@ static void b2_hff_propagate_past(struct HfilterFloat *hff_past)
} else if (past_save_counter == SAVE_NOW) {
/* next GPS measurement valid at this state -> save */
PRINT_DBG(2, ("save past state\n"));
- b2_hff_rb_put_state(hff_past);
+ hff_rb_put_state(hff_past);
past_save_counter = SAVING;
} else if (past_save_counter == SAVING) {
/* increase lag counter on if next state is already saved */
- if (hff_past == &b2_hff_rb[HFF_RB_MAXN - 1]) {
- b2_hff_rb[0].lag_counter++;
+ if (hff_past == &hff_rb[HFF_RB_MAXN - 1]) {
+ hff_rb[0].lag_counter++;
} else {
(hff_past + 1)->lag_counter++;
}
@@ -457,8 +463,8 @@ static void b2_hff_propagate_past(struct HfilterFloat *hff_past)
/* finished re-propagating the past values */
if (hff_past->lag_counter == 0) {
- b2_hff_set_state(&b2_hff_state, hff_past);
- b2_hff_rb_drop_last();
+ hff_set_state(&hff, hff_past);
+ hff_rb_drop_last();
past_save_counter = SAVE_DONE;
break;
}
@@ -467,71 +473,71 @@ static void b2_hff_propagate_past(struct HfilterFloat *hff_past)
#endif /* GPS_LAG */
-void b2_hff_propagate(void)
+void hff_propagate(void)
{
- if (b2_hff_lost_counter < b2_hff_lost_limit) {
- b2_hff_lost_counter++;
+ if (hff_lost_counter < hff_lost_limit) {
+ hff_lost_counter++;
+ }
+
+ if (hff_speed_lost_counter < hff_lost_limit) {
+ hff_speed_lost_counter++;
}
#ifdef GPS_LAG
/* continue re-propagating to catch up with the present */
- if (b2_hff_rb_last->rollback) {
- b2_hff_propagate_past(b2_hff_rb_last);
+ if (hff_rb_last->rollback) {
+ hff_propagate_past(hff_rb_last);
}
#endif
/* rotate imu accel measurement to body frame and filter */
- struct Int32Vect3 acc_meas_body;
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
- int32_rmat_transp_vmult(&acc_meas_body, body_to_imu_rmat, &imu.accel);
-
struct Int32Vect3 acc_body_filtered;
- acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x);
- acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, acc_meas_body.y);
- acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, acc_meas_body.z);
+ acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, stateGetAccelBody_i()->x);
+ acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, stateGetAccelBody_i()->y);
+ acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, stateGetAccelBody_i()->z);
/* propagate current state if it is time */
- if (b2_hff_ps_counter == HFF_PRESCALER) {
- b2_hff_ps_counter = 1;
- if (b2_hff_lost_counter < b2_hff_lost_limit) {
- struct Int32Vect3 filtered_accel_ltp;
- struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
- int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered);
- b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x);
- b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y);
+ if (hff_ps_counter >= HFF_PRESCALER) {
+ hff_ps_counter = 0;
+ struct Int32Vect3 filtered_accel_ltp;
+ struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
+ int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered);
+ hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x);
+ hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y);
+
#ifdef GPS_LAG
- b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
+ hff_store_accel_ltp(hff_xdd_meas, hff_ydd_meas);
#endif
+ if (hff_lost_counter < hff_lost_limit || hff_speed_lost_counter < hff_lost_limit) {
/*
* propagate current state
*/
- b2_hff_propagate_x(&b2_hff_state, DT_HFILTER);
- b2_hff_propagate_y(&b2_hff_state, DT_HFILTER);
+ hff_propagate_x(&hff, DT_HFILTER);
+ hff_propagate_y(&hff, DT_HFILTER);
#ifdef GPS_LAG
/* increase lag counter on last saved state */
- if (b2_hff_rb_n > 0) {
- b2_hff_rb_last->lag_counter++;
+ if (hff_rb_n > 0) {
+ hff_rb_last->lag_counter++;
}
/* save filter state if needed */
if (save_counter == 0) {
PRINT_DBG(1, ("save current state\n"));
- b2_hff_rb_put_state(&b2_hff_state);
+ hff_rb_put_state(&hff);
save_counter = -1;
} else if (save_counter > 0) {
save_counter--;
}
#endif
}
- } else {
- b2_hff_ps_counter++;
}
+ hff_ps_counter++;
}
-void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
+void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned __attribute__((unused)))
{
- b2_hff_lost_counter = 0;
+ hff_lost_counter = 0;
#if USE_GPS_ACC4R
Rgps_pos = (float) gps.pacc / 100.;
@@ -540,8 +546,8 @@ void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
}
Rgps_vel = (float) gps.sacc / 100.;
- if (Rgps_vel < HFF_R_SPEED_MIN) {
- Rgps_vel = HFF_R_SPEED_MIN;
+ if (Rgps_vel < HFF_R_GPS_SPEED_MIN) {
+ Rgps_vel = HFF_R_GPS_SPEED_MIN;
}
#endif
@@ -550,56 +556,57 @@ void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
#endif
/* update filter state with measurement */
- b2_hff_update_x(&b2_hff_state, pos_ned->x, Rgps_pos);
- b2_hff_update_y(&b2_hff_state, pos_ned->y, Rgps_pos);
-#if HFF_UPDATE_SPEED
- b2_hff_update_xdot(&b2_hff_state, speed_ned->x, Rgps_vel);
- b2_hff_update_ydot(&b2_hff_state, speed_ned->y, Rgps_vel);
+ hff_update_x(&hff, pos_ned->x, Rgps_pos);
+ hff_update_y(&hff, pos_ned->y, Rgps_pos);
+#if HFF_UPDATE_GPS_SPEED
+ hff_update_xdot(&hff, speed_ned->x, Rgps_vel);
+ hff_update_ydot(&hff, speed_ned->y, Rgps_vel);
#endif
#ifdef GPS_LAG
- } else if (b2_hff_rb_n > 0) {
+ } else if (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;
- PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter,
+ lag_counter_err = hff_rb_last->lag_counter - GPS_LAG_N;
+ PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", hff_rb_n, hff_rb_last->lag_counter,
lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
- b2_hff_rb_last->rollback = true;
- b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos);
- b2_hff_update_y(b2_hff_rb_last, pos_ned->y, Rgps_pos);
-#if HFF_UPDATE_SPEED
- b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel);
- b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel);
+ hff_rb_last->rollback = true;
+ hff_update_x(hff_rb_last, pos_ned->x, Rgps_pos);
+ hff_update_y(hff_rb_last, pos_ned->y, Rgps_pos);
+#if HFF_UPDATE_GPS_SPEED
+ hff_update_xdot(hff_rb_last, speed_ned->x, Rgps_vel);
+ hff_update_ydot(hff_rb_last, speed_ned->y, Rgps_vel);
#endif
past_save_counter = GPS_DT_N - 1; // + lag_counter_err;
PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
- b2_hff_propagate_past(b2_hff_rb_last);
+ hff_propagate_past(hff_rb_last);
} else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N + 1)) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
- b2_hff_rb_drop_last();
- b2_hff_update_gps(pos_ned, speed_ned);
+ hff_rb_drop_last();
+ hff_update_gps(pos_ned, speed_ned);
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N - 1 - (GPS_LAG_N % GPS_DT_N);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
-
#endif /* GPS_LAG */
}
-void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
+void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
{
- b2_hff_state.x = pos.x;
- b2_hff_state.y = pos.y;
- b2_hff_state.xdot = vel.x;
- b2_hff_state.ydot = vel.y;
+ hff.x = pos.x;
+ hff.y = pos.y;
+ hff.xdot = vel.x;
+ hff.ydot = vel.y;
+ hff.xbias = 0.f;
+ hff.ybias = 0.f;
#ifdef GPS_LAG
- while (b2_hff_rb_n > 0) {
- b2_hff_rb_drop_last();
+ while (hff_rb_n > 0) {
+ hff_rb_drop_last();
}
save_counter = -1;
past_save_counter = SAVE_DONE;
@@ -607,131 +614,172 @@ void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
}
-/*
+/**
+ * Propagate the filter in time.
*
- * Propagation
+ * F = [ 1 dt -dt^2/2
+ * 0 1 -dt
+ * 0 0 1 ];
*
+ * B = [ dt^2/2 dt 0]';
*
-
- 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 void b2_hff_propagate_x(struct HfilterFloat *hff_work, float dt)
+ * Q = [ Q 0 0
+ * 0 Qdotdot 0
+ * 0 0 Qbiasbias ];
+ *
+ * Xk1 = F * Xk0 + B * accel;
+ *
+ * Pk1 = F * Pk0 * F' + Q;
+ *
+ */
+static void hff_propagate_x(struct HfilterFloat *filt, float dt)
{
/* update state */
- hff_work->xdotdot = b2_hff_xdd_meas;
- hff_work->x = hff_work->x + dt * hff_work->xdot + dt * dt / 2 * hff_work->xdotdot;
- hff_work->xdot = hff_work->xdot + dt * hff_work->xdotdot;
+ filt->xdotdot = hff_xdd_meas - filt->xbias;
+ filt->x = filt->x + filt->xdot * dt;// + filt->xdotdot * dt * dt / 2;
+ filt->xdot = filt->xdot + dt * filt->xdotdot;
/* update covariance */
- const float FPF00 = hff_work->xP[0][0] + dt * (hff_work->xP[1][0] + hff_work->xP[0][1] + dt * hff_work->xP[1][1]);
- const float FPF01 = hff_work->xP[0][1] + dt * hff_work->xP[1][1];
- const float FPF10 = hff_work->xP[1][0] + dt * hff_work->xP[1][1];
- const float FPF11 = hff_work->xP[1][1];
+ const float FPF00 = filt->xP[0][0] + dt * (filt->xP[1][0] + filt->xP[0][1] + dt * filt->xP[1][1]);
+ const float FPF01 = filt->xP[0][1] + dt * (filt->xP[1][1] - filt->xP[0][2] - dt * filt->xP[1][2]);
+ const float FPF02 = filt->xP[0][2] + dt * (filt->xP[1][2]);
+ const float FPF10 = filt->xP[1][0] + dt * (-filt->xP[2][0] + filt->xP[1][1] - dt * filt->xP[2][1]);
+ const float FPF11 = filt->xP[1][1] + dt * (-filt->xP[2][1] - filt->xP[1][2] + dt * filt->xP[2][2]);
+ const float FPF12 = filt->xP[1][2] + dt * (-filt->xP[2][2]);
+ const float FPF20 = filt->xP[2][0] + dt * (filt->xP[2][1]);
+ const float FPF21 = filt->xP[2][1] + dt * (-filt->xP[2][2]);
+ const float FPF22 = filt->xP[2][2];
- 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;
+ filt->xP[0][0] = FPF00 + Q * dt * dt / 2.;
+ filt->xP[0][1] = FPF01;
+ filt->xP[0][2] = FPF02;
+ filt->xP[1][0] = FPF10;
+ filt->xP[1][1] = FPF11 + Qdotdot * dt;
+ filt->xP[1][2] = FPF12;
+ filt->xP[2][0] = FPF20;
+ filt->xP[2][1] = FPF21;
+ filt->xP[2][2] = FPF22 + Qbiasbias;
}
-static void b2_hff_propagate_y(struct HfilterFloat *hff_work, float dt)
+static void hff_propagate_y(struct HfilterFloat *filt, float dt)
{
/* update state */
- hff_work->ydotdot = b2_hff_ydd_meas;
- hff_work->y = hff_work->y + dt * hff_work->ydot + dt * dt / 2 * hff_work->ydotdot;
- hff_work->ydot = hff_work->ydot + dt * hff_work->ydotdot;
+ filt->ydotdot = hff_ydd_meas - filt->ybias;
+ filt->y = filt->y + dt * filt->ydot;// + filt->ydotdot * dt * dt / 2;
+ filt->ydot = filt->ydot + dt * filt->ydotdot;
/* update covariance */
- const float FPF00 = hff_work->yP[0][0] + dt * (hff_work->yP[1][0] + hff_work->yP[0][1] + dt * hff_work->yP[1][1]);
- const float FPF01 = hff_work->yP[0][1] + dt * hff_work->yP[1][1];
- const float FPF10 = hff_work->yP[1][0] + dt * hff_work->yP[1][1];
- const float FPF11 = hff_work->yP[1][1];
+ const float FPF00 = filt->yP[0][0] + dt * (filt->yP[1][0] + filt->yP[0][1] + dt * filt->yP[1][1]);
+ const float FPF01 = filt->yP[0][1] + dt * (filt->yP[1][1] - filt->yP[0][2] - dt * filt->yP[1][2]);
+ const float FPF02 = filt->yP[0][2] + dt * (filt->yP[1][2]);
+ const float FPF10 = filt->yP[1][0] + dt * (-filt->yP[2][0] + filt->yP[1][1] - dt * filt->yP[2][1]);
+ const float FPF11 = filt->yP[1][1] + dt * (-filt->yP[2][1] - filt->yP[1][2] + dt * filt->yP[2][2]);
+ const float FPF12 = filt->yP[1][2] + dt * (-filt->yP[2][2]);
+ const float FPF20 = filt->yP[2][0] + dt * (filt->yP[2][1]);
+ const float FPF21 = filt->yP[2][1] + dt * (-filt->yP[2][2]);
+ const float FPF22 = filt->yP[2][2];
- 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;
+ filt->yP[0][0] = FPF00 + Q * dt * dt / 2.;
+ filt->yP[0][1] = FPF01;
+ filt->yP[0][2] = FPF02;
+ filt->yP[1][0] = FPF10;
+ filt->yP[1][1] = FPF11 + Qdotdot * dt;
+ filt->yP[1][2] = FPF12;
+ filt->yP[2][0] = FPF20;
+ filt->yP[2][1] = FPF21;
+ filt->yP[2][2] = FPF22 + Qbiasbias;
}
-/*
- *
+/**
* Update position
*
- *
-
- H = [1 0];
- R = 0.1;
- // state residual
- y = pos_measurement - 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 b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
+ * 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 hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
{
- b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x);
- b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y);
+ hff_lost_counter = 0;
+ hff_update_x(&hff, pos.x, Rpos.x);
+ hff_update_y(&hff, pos.y, Rpos.y);
}
-static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos)
+static void hff_update_x(struct HfilterFloat *filt, float x_meas, float Rpos)
{
- b2_hff_x_meas = x_meas;
+ hff_x_meas = x_meas;
- 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;
+ const float y = x_meas - filt->x;
+ const float S = filt->xP[0][0] + Rpos;
+ const float K1 = filt->xP[0][0] * 1 / S;
+ const float K2 = filt->xP[1][0] * 1 / S;
+ const float K3 = filt->xP[2][0] * 1 / S;
- hff_work->x = hff_work->x + K1 * y;
- hff_work->xdot = hff_work->xdot + K2 * y;
+ filt->x = filt->x + K1 * y;
+ filt->xdot = filt->xdot + K2 * y;
+ filt->xbias = filt->xbias + K3 * y;
- 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];
+ const float P11 = (1. - K1) * filt->xP[0][0];
+ const float P12 = (1. - K1) * filt->xP[0][1];
+ const float P13 = (1. - K1) * filt->xP[0][2];
+ const float P21 = -K2 * filt->xP[0][0] + filt->xP[1][0];
+ const float P22 = -K2 * filt->xP[0][1] + filt->xP[1][1];
+ const float P23 = -K2 * filt->xP[0][2] + filt->xP[1][2];
+ const float P31 = -K3 * filt->xP[0][0] + filt->xP[2][0];
+ const float P32 = -K3 * filt->xP[0][1] + filt->xP[2][1];
+ const float P33 = -K3 * filt->xP[0][2] + filt->xP[2][2];
- hff_work->xP[0][0] = P11;
- hff_work->xP[0][1] = P12;
- hff_work->xP[1][0] = P21;
- hff_work->xP[1][1] = P22;
+ filt->xP[0][0] = P11;
+ filt->xP[0][1] = P12;
+ filt->xP[0][2] = P13;
+ filt->xP[1][0] = P21;
+ filt->xP[1][1] = P22;
+ filt->xP[1][2] = P23;
+ filt->xP[2][0] = P31;
+ filt->xP[2][1] = P32;
+ filt->xP[2][2] = P33;
}
-static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos)
+static void hff_update_y(struct HfilterFloat *filt, float y_meas, float Rpos)
{
- b2_hff_y_meas = y_meas;
+ hff_y_meas = y_meas;
- 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;
+ const float y = y_meas - filt->y;
+ const float S = filt->yP[0][0] + Rpos;
+ const float K1 = filt->yP[0][0] * 1 / S;
+ const float K2 = filt->yP[1][0] * 1 / S;
+ const float K3 = filt->yP[2][0] * 1 / S;
- hff_work->y = hff_work->y + K1 * y;
- hff_work->ydot = hff_work->ydot + K2 * y;
+ filt->y = filt->y + K1 * y;
+ filt->ydot = filt->ydot + K2 * y;
+ filt->ybias = filt->ybias + K3 * y;
- 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];
+ const float P11 = (1. - K1) * filt->yP[0][0];
+ const float P12 = (1. - K1) * filt->yP[0][1];
+ const float P13 = (1. - K1) * filt->yP[0][2];
+ const float P21 = -K2 * filt->yP[0][0] + filt->yP[1][0];
+ const float P22 = -K2 * filt->yP[0][1] + filt->yP[1][1];
+ const float P23 = -K2 * filt->yP[0][2] + filt->yP[1][2];
+ const float P31 = -K3 * filt->yP[0][0] + filt->yP[2][0];
+ const float P32 = -K3 * filt->yP[0][1] + filt->yP[2][1];
+ const float P33 = -K3 * filt->yP[0][2] + filt->yP[2][2];
- hff_work->yP[0][0] = P11;
- hff_work->yP[0][1] = P12;
- hff_work->yP[1][0] = P21;
- hff_work->yP[1][1] = P22;
+ filt->yP[0][0] = P11;
+ filt->yP[0][1] = P12;
+ filt->yP[0][2] = P13;
+ filt->yP[1][0] = P21;
+ filt->yP[1][1] = P22;
+ filt->yP[1][2] = P23;
+ filt->yP[2][0] = P31;
+ filt->yP[2][1] = P32;
+ filt->yP[2][2] = P33;
}
@@ -739,69 +787,92 @@ static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float R
*
* Update velocity
*
- *
-
- H = [0 1];
- R = 0.1;
- // state residual
- yd = vx - 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;
+ * H = [0 1 0];
+ * R = 0.1;
+ * // state residual
+ * yd = vx - 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 b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
+void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
{
- b2_hff_update_xdot(&b2_hff_state, vel.x, Rvel.x);
- b2_hff_update_ydot(&b2_hff_state, vel.y, Rvel.y);
+ hff_speed_lost_counter = 0;
+ hff_update_xdot(&hff, vel.x, Rvel.x);
+ hff_update_ydot(&hff, vel.y, Rvel.y);
}
-static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel)
+static void hff_update_xdot(struct HfilterFloat *filt, float vel, float Rvel)
{
- b2_hff_xd_meas = vel;
+ hff_xd_meas = vel;
- const float yd = vel - hff_work->xdot;
- const float S = hff_work->xP[1][1] + Rvel;
- const float K1 = hff_work->xP[0][1] * 1 / S;
- const float K2 = hff_work->xP[1][1] * 1 / S;
+ const float yd = vel - filt->xdot;
+ const float S = filt->xP[1][1] + Rvel;
+ const float K1 = filt->xP[0][1] * 1 / S;
+ const float K2 = filt->xP[1][1] * 1 / S;
+ const float K3 = filt->xP[2][1] * 1 / S;
- hff_work->x = hff_work->x + K1 * yd;
- hff_work->xdot = hff_work->xdot + K2 * yd;
+ filt->x = filt->x + K1 * yd;
+ filt->xdot = filt->xdot + K2 * yd;
+ filt->xbias = filt->xbias + K3 * yd;
- 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];
+ const float P11 = -K1 * filt->xP[1][0] + filt->xP[0][0];
+ const float P12 = -K1 * filt->xP[1][1] + filt->xP[0][1];
+ const float P13 = -K1 * filt->xP[1][2] + filt->xP[0][2];
+ const float P21 = (1. - K2) * filt->xP[1][0];
+ const float P22 = (1. - K2) * filt->xP[1][1];
+ const float P23 = (1. - K2) * filt->xP[1][2];
+ const float P31 = -K3 * filt->xP[1][0] + filt->xP[2][0];
+ const float P32 = -K3 * filt->xP[1][1] + filt->xP[2][1];
+ const float P33 = -K3 * filt->xP[1][2] + filt->xP[2][2];
- hff_work->xP[0][0] = P11;
- hff_work->xP[0][1] = P12;
- hff_work->xP[1][0] = P21;
- hff_work->xP[1][1] = P22;
+ filt->xP[0][0] = P11;
+ filt->xP[0][1] = P12;
+ filt->xP[0][2] = P13;
+ filt->xP[1][0] = P21;
+ filt->xP[1][1] = P22;
+ filt->xP[1][2] = P23;
+ filt->xP[2][0] = P31;
+ filt->xP[2][1] = P32;
+ filt->xP[2][2] = P33;
}
-static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel)
+static void hff_update_ydot(struct HfilterFloat *filt, float vel, float Rvel)
{
- b2_hff_yd_meas = vel;
+ hff_yd_meas = vel;
- const float yd = vel - hff_work->ydot;
- const float S = hff_work->yP[1][1] + Rvel;
- const float K1 = hff_work->yP[0][1] * 1 / S;
- const float K2 = hff_work->yP[1][1] * 1 / S;
+ const float yd = vel - filt->ydot;
+ const float S = filt->yP[1][1] + Rvel;
+ const float K1 = filt->yP[0][1] * 1 / S;
+ const float K2 = filt->yP[1][1] * 1 / S;
+ const float K3 = filt->yP[2][1] * 1 / S;
- hff_work->y = hff_work->y + K1 * yd;
- hff_work->ydot = hff_work->ydot + K2 * yd;
+ filt->y = filt->y + K1 * yd;
+ filt->ydot = filt->ydot + K2 * yd;
+ filt->ybias = filt->ybias + K3 * yd;
- 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];
+ const float P11 = -K1 * filt->yP[1][0] + filt->yP[0][0];
+ const float P12 = -K1 * filt->yP[1][1] + filt->yP[0][1];
+ const float P13 = -K1 * filt->yP[1][2] + filt->yP[0][2];
+ const float P21 = (1. - K2) * filt->yP[1][0];
+ const float P22 = (1. - K2) * filt->yP[1][1];
+ const float P23 = (1. - K2) * filt->yP[1][2];
+ const float P31 = -K3 * filt->yP[1][0] + filt->yP[2][0];
+ const float P32 = -K3 * filt->yP[1][1] + filt->yP[2][1];
+ const float P33 = -K3 * filt->yP[1][2] + filt->yP[2][2];
- hff_work->yP[0][0] = P11;
- hff_work->yP[0][1] = P12;
- hff_work->yP[1][0] = P21;
- hff_work->yP[1][1] = P22;
+ filt->yP[0][0] = P11;
+ filt->yP[0][1] = P12;
+ filt->yP[0][2] = P13;
+ filt->yP[1][0] = P21;
+ filt->yP[1][1] = P22;
+ filt->yP[1][2] = P23;
+ filt->yP[2][0] = P31;
+ filt->yP[2][1] = P32;
+ filt->yP[2][2] = P33;
}
diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h
index 8491178f63..eaacf8159f 100644
--- a/sw/airborne/subsystems/ins/hf_float.h
+++ b/sw/airborne/subsystems/ins/hf_float.h
@@ -31,32 +31,34 @@
#include "std.h"
#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"
-#define HFF_STATE_SIZE 2
+// X = [ z zdot bias ]
+#define HFF_STATE_SIZE 3
struct HfilterFloat {
float x;
- /* float xbias; */
float xdot;
float xdotdot;
+ float xbias;
float y;
- /* float ybias; */
float ydot;
float ydotdot;
+ float ybias;
float xP[HFF_STATE_SIZE][HFF_STATE_SIZE];
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE];
uint16_t lag_counter;
bool rollback;
};
-extern struct HfilterFloat b2_hff_state;
+extern struct HfilterFloat hff;
-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(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned);
-extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos);
-extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
-extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
+extern void hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
+extern void hff_propagate(void);
+extern void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned);
+extern void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos);
+extern void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
+extern void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
#endif /* HF_FLOAT_H */
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 23bc3353e5..b4357b6b08 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -143,7 +143,18 @@ static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
#define INS_INT_VEL_ID ABI_BROADCAST
#endif
static abi_event vel_est_ev;
-static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise);
+static void vel_est_cb(uint8_t sender_id,
+ uint32_t stamp,
+ float x, float y, float z,
+ float noise);
+#ifndef INS_INT_POS_ID
+#define INS_INT_POS_ID ABI_BROADCAST
+#endif
+static abi_event pos_est_ev;
+static void pos_est_cb(uint8_t sender_id,
+ uint32_t stamp,
+ float x, float y, float z,
+ float noise);
struct InsInt ins_int;
@@ -211,7 +222,7 @@ void ins_int_init(void)
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
- b2_hff_init(0., 0., 0., 0.);
+ hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_int.ltp_pos);
@@ -230,6 +241,7 @@ void ins_int_init(void)
AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
+ AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
}
void ins_reset_local_origin(void)
@@ -298,7 +310,7 @@ void ins_int_propagate(struct Int32Vect3 *accel, float dt)
#if USE_HFF
/* propagate horizontal filter */
- b2_hff_propagate();
+ hff_propagate();
/* convert and copy result to ins_int */
ins_update_from_hff();
#else
@@ -333,11 +345,11 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
// Calculate the distance to the origin
struct EnuCoor_f *enu = stateGetPositionEnu_f();
- double dist2_to_origin = enu->x*enu->x + enu->y*enu->y;
+ double dist2_to_origin = enu->x * enu->x + enu->y * enu->y;
// correction for the earth's curvature
const double earth_radius = 6378137.0;
- float height_correction = (float) (sqrt(earth_radius*earth_radius + dist2_to_origin) - earth_radius);
+ float height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
// The VFF will update in the NED frame
ins_int.baro_z = -(baro_up - height_correction);
@@ -406,15 +418,14 @@ void ins_int_update_gps(struct GpsState *gps_s)
struct FloatVect2 gps_speed_m_s_ned;
VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
- VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
+ VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.f);
if (ins_int.hf_realign) {
ins_int.hf_realign = false;
- const struct FloatVect2 zero = {0.0f, 0.0f};
- b2_hff_realign(gps_pos_m_ned, zero);
+ hff_realign(gps_pos_m_ned, gps_speed_m_s_ned);
}
// run horizontal filter
- b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
+ hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
// convert and copy result to ins_int
ins_update_from_hff();
@@ -489,12 +500,12 @@ static void ins_update_from_vff(void)
/** update ins state from horizontal filter */
static void ins_update_from_hff(void)
{
- ins_int.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
- ins_int.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
- ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
- ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
- ins_int.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
- ins_int.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+ ins_int.ltp_accel.x = ACCEL_BFP_OF_REAL(hff.xdotdot);
+ ins_int.ltp_accel.y = ACCEL_BFP_OF_REAL(hff.ydotdot);
+ ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(hff.xdot);
+ ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(hff.ydot);
+ ins_int.ltp_pos.x = POS_BFP_OF_REAL(hff.x);
+ ins_int.ltp_pos.y = POS_BFP_OF_REAL(hff.y);
}
#endif
@@ -520,15 +531,15 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
ins_int_update_gps(gps_s);
}
+/* body relative velocity estimate
+ *
+ */
static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp,
+ uint32_t stamp __attribute__((unused)),
float x, float y, float z,
- float noise __attribute__((unused)))
+ float noise)
{
-
struct FloatVect3 vel_body = {x, y, z};
- static uint32_t last_stamp = 0;
- float dt = 0;
/* rotate velocity estimate to nav/ltp frame */
struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
@@ -536,29 +547,53 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
struct FloatVect3 vel_ned;
float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
- if (last_stamp > 0) {
- dt = (float)(stamp - last_stamp) * 1e-6;
- }
-
- last_stamp = stamp;
-
#if USE_HFF
- (void)dt; //dt is unused variable in this define
-
struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
struct FloatVect2 Rvel = {noise, noise};
- b2_hff_update_vel(vel, Rvel);
+ hff_update_vel(vel, Rvel);
ins_update_from_hff();
#else
ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
+
+ static uint32_t last_stamp = 0;
if (last_stamp > 0) {
+ float dt = (float)(stamp - last_stamp) * 1e-6;
ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x);
ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y);
}
+ last_stamp = stamp;
#endif
+ vff_update_vz_conf(vel_ned.z, noise);
+
+ ins_ned_to_state();
+
+ /* reset the counter to indicate we just had a measurement update */
+ ins_int.propagation_cnt = 0;
+}
+
+/* NED position estimate relative to ltp origin
+ */
+static void pos_est_cb(uint8_t sender_id __attribute__((unused)),
+ uint32_t stamp __attribute__((unused)),
+ float x, float y, float z,
+ float noise)
+{
+#if USE_HFF
+ struct FloatVect2 pos = {x, y};
+ struct FloatVect2 Rpos = {noise, noise};
+
+ hff_update_pos(pos, Rpos);
+ ins_update_from_hff();
+#else
+ ins_int.ltp_pos.x = POS_BFP_OF_REAL(x);
+ ins_int.ltp_pos.y = POS_BFP_OF_REAL(y);
+#endif
+
+ vff_update_z_conf(z, noise);
+
ins_ned_to_state();
/* reset the counter to indicate we just had a measurement update */
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index a25a5e9463..39e4d1c66a 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit a25a5e9463c138459f65b3b8d46e14312ac5774c
+Subproject commit 39e4d1c66af4d2cc3f319de7ce4b7ec0e358b961