Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

hfilter: take gps lag into account.

Browse Source 
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.
This commit is contained in:
Felix Ruess
2009-08-31 18:46:26 +00:00
parent 2c676750e4
commit 35691e9262
9 changed files with 220 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_flixr.xml
+1 -1
View File
@@ -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>
conf/messages.xml
+6
View File
@@ -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"/>
conf/telemetry/telemetry_booz2.xml
+1
View File
@@ -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">
conf/telemetry/telemetry_booz2_flixr.xml
+1
View File
@@ -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">
sw/airborne/booz/booz2_ins.c
+11 -12
View File
@@ -52,6 +52,9 @@ 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;
/* 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 @@ void booz_ins_propagate() {
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 @@ 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);
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;
sw/airborne/booz/booz2_ins.h
+4
View File
@@ -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 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;
/* 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 );
sw/airborne/booz/booz2_telemetry.h
+7
View File
@@ -476,9 +476,16 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
& 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) { \
sw/airborne/booz/ins/booz2_hf_float.c
+179 -11
View File
@@ -23,6 +23,8 @@
*/
#include "booz2_hf_float.h"
#include "booz2_ins.h"
#include <stdlib.h>
/*
@@ -63,6 +65,52 @@ float b2_hff_ydotdot;
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 @@ 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(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 @@ static inline void b2_hff_update_ydot(float v);
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,9 +170,71 @@ static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias
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
/*
@@ -137,13 +254,30 @@ static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias
*/
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_x(float xaccel) {
}
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,6 +332,40 @@ static inline void b2_hff_propagate_y(float yaccel) {
}
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];
sw/airborne/booz/ins/booz2_hf_float.h
+10
View File
@@ -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 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_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 */