Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

use gps accuracy information for measurement noise in hfilter.

Browse Source 
define USE_GPS4R to use it
This commit is contained in:
Felix Ruess
2009-09-02 12:09:38 +00:00
parent d999a3a982
commit 43a5e7a36a
2 changed files with 25 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_flixr.xml
+1 -1
View File
@@ -205,7 +205,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8
ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
</makefile>
sw/airborne/booz/ins/booz2_hf_float.c
+24 -2
View File
@@ -26,6 +26,7 @@
#include "booz2_ins.h"
#include "booz_imu.h"
#include "booz_ahrs.h"
#include "booz2_gps.h"
#include <stdlib.h>
#ifdef SITL
@@ -47,8 +48,14 @@
#define Q ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
#define Qdotdot ACCEL_NOISE*DT_HFILTER
//TODO: proper measurement noise
#define Rpos 5.
#define Rspeed 1.
#define R_POS 7.
#define R_POS_MAX 20.
#define R_POS_MIN 2.
#define R_SPEED 2.
#define R_SPEED_MAX 10.
#define R_SPEED_MIN 0.2
float Rpos, Rspeed;
/*
@@ -162,6 +169,8 @@ 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) {
Rpos = R_POS;
Rspeed = R_SPEED;
b2_hff_init_x(init_x, init_xdot);
b2_hff_init_y(init_y, init_ydot);
#ifdef GPS_LAG
@@ -390,6 +399,19 @@ void b2_hff_propagate(void) {
void b2_hff_update_gps(void) {
#ifdef USE_GPS_ACC4R
Rpos = (float) booz_gps_state.pacc / 100.;
if (Rpos > R_POS_MAX)
Rpos = R_POS_MAX;
else if (Rpos < R_POS_MIN)
Rpos = R_POS_MIN;
Rspeed = (float) booz_gps_state.sacc / 100.;
if (Rspeed > R_SPEED_MAX)
Rspeed = R_SPEED_MAX;
else if (Rspeed < R_SPEED_MIN)
Rspeed = R_SPEED_MIN;
#endif
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif