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

[ahrs] float_mlkf: configurable mag measurement noise

Browse Source
This commit is contained in:
Felix Ruess
2013-10-31 17:34:46 +01:00
parent c9fa0b4692
commit 84cf87cfa5
3 changed files with 34 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/estimation/ahrs_float_mlkf.xml
+13
View File
@@ -0,0 +1,13 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
<dl_setting var="ahrs_impl.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
<dl_setting var="ahrs_impl.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+19 -9
View File
@@ -50,9 +50,15 @@
#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
#endif #endif
#ifndef AHRS_MAG_NOISE_X
#define AHRS_MAG_NOISE_X 0.2
#define AHRS_MAG_NOISE_Y 0.2
#define AHRS_MAG_NOISE_Z 0.2
#endif
static inline void propagate_ref(void); static inline void propagate_ref(void);
static inline void propagate_state(void); static inline void propagate_state(void);
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]); static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise);
static inline void reset_state(void); static inline void reset_state(void);
static inline void set_body_state_from_quat(void); static inline void set_body_state_from_quat(void);
@@ -93,6 +99,8 @@ void ahrs_init(void) {
{ 0., 0., 0., 0., 0., P0_b}}; { 0., 0., 0., 0., 0., P0_b}};
memcpy(ahrs_impl.P, P0, sizeof(P0)); memcpy(ahrs_impl.P, P0, sizeof(P0));
VECT3_ASSIGN(ahrs_impl.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
} }
void ahrs_align(void) { void ahrs_align(void) {
@@ -128,9 +136,8 @@ void ahrs_update_accel(void) {
(1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81); (1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81);
const struct FloatVect3 earth_g = {0., 0., -9.81 }; const struct FloatVect3 earth_g = {0., 0., -9.81 };
const float dn = 250*fabs( ahrs_impl.lp_accel ); const float dn = 250*fabs( ahrs_impl.lp_accel );
const float g_noise[] = {1.+dn, 1.+dn, 1.+dn}; struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
// const float g_noise[] = {150., 150., 150.}; update_state(&earth_g, &imu_g, &g_noise);
update_state(&earth_g, &imu_g, g_noise);
reset_state(); reset_state();
} }
@@ -138,8 +145,7 @@ void ahrs_update_accel(void) {
void ahrs_update_mag(void) { void ahrs_update_mag(void) {
struct FloatVect3 imu_h; struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, imu.mag); MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
const float h_noise[] = { 0.1610, 0.1771, 0.2659}; update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise);
update_state(&ahrs_impl.mag_h, &imu_h, h_noise);
reset_state(); reset_state();
} }
@@ -200,7 +206,7 @@ static inline void propagate_state(void) {
/** /**
* Incorporate one 3D vector measurement * Incorporate one 3D vector measurement
*/ */
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, const float noise[]) { static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) {
/* converted expected measurement from inertial to body frame */ /* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected; struct FloatVect3 b_expected;
@@ -214,8 +220,12 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
MAT_MUL(3,6,6, tmp, H, ahrs_impl.P); MAT_MUL(3,6,6, tmp, H, ahrs_impl.P);
float S[3][3]; float S[3][3];
MAT_MUL_T(3,6,3, S, tmp, H); MAT_MUL_T(3,6,3, S, tmp, H);
for (int i=0;i<3;i++)
S[i][i] += noise[i]; /* add the measurement noise */
S[0][0] += noise->x;
S[1][1] += noise->y;
S[2][2] += noise->z;
float invS[3][3]; float invS[3][3];
MAT_INV33(invS, S); MAT_INV33(invS, S);
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+2
View File
@@ -43,6 +43,8 @@ struct AhrsMlkf {
struct FloatVect3 mag_h; struct FloatVect3 mag_h;
struct FloatVect3 mag_noise;
struct FloatQuat gibbs_cor; struct FloatQuat gibbs_cor;
float P[6][6]; float P[6][6];
float lp_accel; float lp_accel;