From 92f0032475321c80a06ddbeaeb1f16d3d8e24bd8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 30 Jan 2016 22:35:20 +1100 Subject: [PATCH] ekf2: Add tuning parameters for GPS and Baro innovation gates --- src/modules/ekf2/ekf2_main.cpp | 6 ++++++ src/modules/ekf2/ekf2_params.c | 21 +++++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index c7b5723d1f..034cf4d4fb 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -162,6 +162,9 @@ private: control::BlockParamFloat *_gps_vel_noise; control::BlockParamFloat *_gps_pos_noise; control::BlockParamFloat *_baro_noise; + control::BlockParamFloat *_baro_innov_gate; // innovation gate for barometric height innovation test (std dev) + control::BlockParamFloat *_posNE_innov_gate; // innovation gate for GPS horizontal position innovation test (std dev) + control::BlockParamFloat *_vel_innov_gate; // innovation gate for GPS velocity innovation test (std dev) control::BlockParamFloat *_mag_heading_noise; // measurement noise used for simple heading fusion control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees @@ -214,6 +217,9 @@ Ekf2::Ekf2(): _gps_vel_noise = new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, ¶ms->gps_vel_noise); _gps_pos_noise = new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, ¶ms->gps_pos_noise); _baro_noise = new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, ¶ms->baro_noise); + _baro_innov_gate = new control::BlockParamFloat(this, "EKF2_BARO_GATE", false, ¶ms->baro_innov_gate); + _posNE_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, ¶ms->posNE_innov_gate); + _vel_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, ¶ms->vel_innov_gate); _mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, ¶ms->mag_heading_noise); _mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, ¶ms->mag_declination_deg); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index b900ebdca9..30912b055e 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -300,3 +300,24 @@ PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f); * @group EKF2 */ PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f); + +/** + * Gate size for barometric height fusion (standard deviations) + * + * @group EKF2 + */ +PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f); + +/** + * Gate size for GPS horizontal position fusion (standard deviations) + * + * @group EKF2 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f); + +/** + * Gate size for GPS velocity fusion (standard deviations) + * + * @group EKF2 + */ +PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f);