[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4033] hfilter: take gps lag into account.
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [4033] hfilter: take gps lag into account. |
|
Date: |
Mon, 31 Aug 2009 18:46:30 +0000 |
Revision: 4033
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4033
Author: flixr
Date: 2009-08-31 18:46:26 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
hfilter: take gps lag into account.
GPS update is performed on state saved GPS_LAG seconds ago. Rerun all
propagation steps since GPS update. Rerunning the propagation steps
still need to be spread over several cycles.
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/booz2_flixr.xml
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
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/conf/airframes/booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2009-08-31 18:46:08 UTC
(rev 4032)
+++ paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2009-08-31 18:46:26 UTC
(rev 4033)
@@ -204,7 +204,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
#enable horizontal filter
- ap.CFLAGS += -DUSE_HFF -DHFF_PRESCALER=16
+ ap.CFLAGS += -DUSE_HFF -DHFF_PRESCALER=16 -DGPS_LAG=0.2
</makefile>
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2009-08-31 18:46:08 UTC (rev 4032)
+++ paparazzi3/trunk/conf/messages.xml 2009-08-31 18:46:26 UTC (rev 4033)
@@ -1086,6 +1086,12 @@
<field name="Pbb" type="float"/>
</message>
+ <message name="BOOZ2_HFF_GPS" id="166">
+ <field name="lag_cnt" type="uint8"/>
+ <field name="lag_cnt_err" type="int8"/>
+ <field name="save_cnt" type="int8"/>
+ </message>
+
<message name="EKF7_XHAT" id="170">
<field name="c" type="float"/>
<field name="s1" type="float"/>
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2009-08-31 18:46:08 UTC
(rev 4032)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2009-08-31 18:46:26 UTC
(rev 4033)
@@ -89,6 +89,7 @@
<message name="BOOZ2_NAV_REF" period="5."/>
<message name="BOOZ2_HFF_X" period=".05"/>
<message name="BOOZ2_HFF_Y" period=".05"/>
+ <message name="BOOZ2_HFF_GPS" period=".03"/>
</mode>
<mode name="aligner">
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2009-08-31
18:46:08 UTC (rev 4032)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2009-08-31
18:46:26 UTC (rev 4033)
@@ -89,6 +89,7 @@
<message name="BOOZ2_NAV_REF" period="5."/>
<message name="BOOZ2_HFF_X" period=".05"/>
<message name="BOOZ2_HFF_Y" period=".05"/>
+ <message name="BOOZ2_HFF_GPS" period=".03"/>
</mode>
<mode name="aligner">
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:46:08 UTC
(rev 4032)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:46:26 UTC
(rev 4033)
@@ -52,6 +52,9 @@
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;
+/* 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;
/* barometer */
#ifdef USE_VFF
@@ -143,6 +146,9 @@
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);
@@ -202,27 +208,20 @@
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);
+ 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
- struct FloatVect2 gps_float;
-#ifdef B2_HFF_UPDATE_POS
- VECT2_ASSIGN(gps_float, booz_ins_gps_pos_cm_ned.x,
booz_ins_gps_pos_cm_ned.y);
- VECT2_SDIV(gps_float, gps_float, 100.);
- b2_hff_update_pos(gps_float.x, gps_float.y);
-#endif
-#ifdef B2_HFF_UPDATE_SPEED
- VECT2_ASSIGN(gps_float, booz_ins_gps_speed_cm_s_ned.x,
booz_ins_gps_speed_cm_s_ned.y);
- VECT2_SDIV(gps_float, gps_float, 100.);
- b2_hff_update_v(gps_float.x, gps_float.y);
-#endif
+ 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);
-
#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_ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2009-08-31 18:46:08 UTC
(rev 4032)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2009-08-31 18:46:26 UTC
(rev 4033)
@@ -26,6 +26,7 @@
#include "std.h"
#include "math/pprz_geodetic_int.h"
+#include "math/pprz_algebra_float.h"
/* gps transformed to LTP-NED */
extern struct LtpDef_i booz_ins_ltp_def;
@@ -49,6 +50,9 @@
extern struct EnuCoor_i booz_ins_enu_pos;
extern struct EnuCoor_i booz_ins_enu_speed;
extern struct EnuCoor_i booz_ins_enu_accel;
+/* 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;
extern void booz_ins_init( void );
extern void booz_ins_periodic( void );
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-08-31 18:46:08 UTC
(rev 4032)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-08-31 18:46:26 UTC
(rev 4033)
@@ -476,9 +476,16 @@
& b2_hff_yP[1][1], \
& b2_hff_yP[2][2]); \
}
+#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
+ DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
+ &lag_counter,
\
+ &lag_counter_err,
\
+ &save_counter);
\
+ }
#else
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) {}
+#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
#endif
#define PERIODIC_SEND_BOOZ2_GUIDANCE(_chan) { \
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:08 UTC (rev 4032)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-08-31
18:46:26 UTC (rev 4033)
@@ -23,6 +23,8 @@
*/
#include "booz2_hf_float.h"
+#include "booz2_ins.h"
+#include <stdlib.h>
/*
@@ -63,6 +65,52 @@
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;
+float b2_hff_yaccel;
+
+/* last position measurement */
float b2_hff_x_meas;
float b2_hff_y_meas;
@@ -70,8 +118,8 @@
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(float xaccel);
-static inline void b2_hff_propagate_y(float yaccel);
+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);
@@ -82,8 +130,15 @@
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_init_x(init_x, init_xdot, init_xbias);
+ b2_hff_init_y(init_y, init_ydot, init_ybias);
+#ifdef GPS_LAG
+ buf_r = 0;
+ buf_w = 0;
+ buf_n = 0;
+ lag_counter = 0;
+ lag_counter_err = 0;
+#endif
}
static inline void b2_hff_init_x(float init_x, float init_xdot, float
init_xbias) {
@@ -115,10 +170,72 @@
else
b2_hff_yP[i][i] = INIT_PXX_BIAS;
}
+}
+#ifdef GPS_LAG
+void b2_hff_store_accel(float x, float y) {
+ past_accel[buf_w].x = x;
+ past_accel[buf_w].y = y;
+ buf_w = (buf_w + 1) < BUF_MAXN ? (buf_w + 1) : 0;
+
+ if (buf_n < BUF_MAXN) {
+ buf_n++;
+ } else {
+ buf_r = (buf_r + 1) < BUF_MAXN ? (buf_r + 1) : 0;
+ }
}
+/* get the accel values from back_n steps ago */
+static inline void b2_hff_get_past_accel(int back_n) {
+ int i;
+ if (back_n > buf_n) {
+ //printf("Cannot go back %d steps, going back only %d instead!\n",
back_n, buf_n);
+ back_n = buf_n;
+ }
+ //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);
+ 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];
+ }
+ }
+}
+
+/* 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;
+ 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];
+ }
+ }
+ lag_counter = 0;
+}
+#endif
+
/*
F = [ 1 dt -dt^2/2
@@ -137,13 +254,30 @@
*/
void b2_hff_propagate(float xaccel, float yaccel) {
- b2_hff_propagate_x(xaccel);
- b2_hff_propagate_y(yaccel);
+#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--;
+ }
+#endif
+
+ b2_hff_xaccel = xaccel;
+ b2_hff_yaccel = yaccel;
+ b2_hff_propagate_x();
+ b2_hff_propagate_y();
+
+#ifdef GPS_LAG
+ if (lag_counter < 2*GPS_LAG_N) // prevent from overflowing if no gps
available
+ lag_counter++;
+#endif
}
-static inline void b2_hff_propagate_x(float xaccel) {
+static inline void b2_hff_propagate_x(void) {
/* update state */
- b2_hff_xdotdot = xaccel - b2_hff_xbias;
+ 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;
/* update covariance */
@@ -169,9 +303,9 @@
}
-static inline void b2_hff_propagate_y(float yaccel) {
+static inline void b2_hff_propagate_y(void) {
/* update state */
- b2_hff_ydotdot = yaccel - b2_hff_ybias;
+ 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;
/* update covariance */
@@ -198,7 +332,41 @@
}
+void b2_hff_update_gps(void) {
+#ifdef GPS_LAG
+ lag_counter_err = 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
+#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);
+#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=lag_counter-1; i >= 0; i--) {
+ b2_hff_get_past_accel(i);
+ b2_hff_propagate_x();
+ b2_hff_propagate_y();
+ }
+ }
+
+ /* reset save counter */
+ save_counter = (int)(HFF_FREQ / 4) % GPS_LAG_N;
+#endif
+}
+
+
/*
H = [1 0 0];
R = 0.1;
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:08 UTC (rev 4032)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h 2009-08-31
18:46:26 UTC (rev 4033)
@@ -24,6 +24,8 @@
#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
+#include <inttypes.h>
+
#define B2_HFF_STATE_SIZE 3
#ifndef HFF_PRESCALER
@@ -51,8 +53,16 @@
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_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;
+extern int8_t lag_counter_err;
+extern int8_t save_counter;
+#endif
+
#endif /* BOOZ2_HF_FLOAT_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4033] hfilter: take gps lag into account.,
Felix Ruess <=