From 4594b3c69d7fd49f7086483f39226d619f143527 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 26 Jan 2016 17:09:00 +1100 Subject: [PATCH] ekf2: Add parameters for magnetometer error checks --- src/modules/ekf2/ekf2_main.cpp | 4 +++- src/modules/ekf2/ekf2_params.c | 11 +++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 7d836c547c..4211be95f2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -166,6 +166,7 @@ private: control::BlockParamFloat *_mag_heading_noise; // measurement noise used for simple heading fusion control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees control::BlockParamFloat *_heading_innov_gate; // innovation gate for heading innovation test + control::BlockParamFloat *_mag_innov_gate; // innovation gate for magnetometer innovation test control::BlockParamInt *_gps_check_mask; // bitmasked integer used to activate the different GPS quality checks control::BlockParamFloat *_requiredEph; // maximum acceptable horiz position error (m) @@ -216,7 +217,8 @@ Ekf2::Ekf2(): _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); - _heading_innov_gate = new control::BlockParamFloat(this, "EKF2_H_INOV_GATE", false, ¶ms->heading_innov_gate); + _heading_innov_gate = new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, ¶ms->heading_innov_gate); + _mag_innov_gate = new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, ¶ms->mag_innov_gate); _gps_check_mask = new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, ¶ms->gps_check_mask); _requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, ¶ms->req_hacc); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 4af3fe00fe..b900ebdca9 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -288,8 +288,15 @@ PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3e-2f); PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); /** - * Gate for maginetic heading fusion + * Gate size for magnetic heading fusion (standard deviations) * * @group EKF2 */ -PARAM_DEFINE_FLOAT(EKF2_H_INOV_GATE, 0.5f); +PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f); + +/** + * Gate size for magnetometer XYZ component fusion (standard deviations) + * + * @group EKF2 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f);