ekf2: Add tuning parameters for GPS and Baro innovation gates

mazgch · Jan 31, 2016 · 92f0032 · 92f0032
1 parent 6ffcca4
commit 92f0032
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 0 deletions.
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
@@ -162,6 +162,9 @@ class Ekf2 : public control::SuperBlock
 	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, &params->gps_vel_noise);
 	_gps_pos_noise = new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, &params->gps_pos_noise);
 	_baro_noise = new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, &params->baro_noise);
+        _baro_innov_gate = new control::BlockParamFloat(this, "EKF2_BARO_GATE", false, &params->baro_innov_gate);
+        _posNE_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, &params->posNE_innov_gate);
+        _vel_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, &params->vel_innov_gate);
 
 	_mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, &params->mag_heading_noise);
 	_mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &params->mag_declination_deg);

diff --git 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);