ekf2: Update tuning parameters

Set conservative defaults as a baseline for tuning
Add a missing parameter for magnetometer observation noise.
Correct error in definition of magnetic heading observations noise (previous parameter defined the variance directly, not the noise).
Update documentation and display names for consistency.

src/modules/ekf2/ekf2_main.cpp

@@ -170,7 +170,8 @@ private:
 	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
+        control::BlockParamFloat *_mag_noise;           // measurement noise used for 3-axis magnetoemter fusion (Gauss)
+        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
 
@@ -201,15 +202,15 @@ Ekf2::Ekf2():
 	_ekf(new Ekf()),
 	_params(_ekf->getParamHandle()),
 	_mag_delay_ms(new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, &_params->mag_delay_ms)),
-	_baro_delay_ms(new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &_params->baro_delay_ms)),
+        _baro_delay_ms(new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &_params->baro_delay_ms)),
 	_gps_delay_ms(new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, &_params->gps_delay_ms)),
 	_airspeed_delay_ms(new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms)),
-	_gyro_noise(new control::BlockParamFloat(this, "EKF2_G_NOISE", false, &_params->gyro_noise)),
-	_accel_noise(new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &_params->accel_noise)),
-	_gyro_bias_p_noise(new control::BlockParamFloat(this, "EKF2_GB_NOISE", false, &_params->gyro_bias_p_noise)),
-	_accel_bias_p_noise(new control::BlockParamFloat(this, "EKF2_ACCB_NOISE", false, &_params->accel_bias_p_noise)),
-	_gyro_scale_p_noise(new control::BlockParamFloat(this, "EKF2_GS_NOISE", false, &_params->gyro_scale_p_noise)),
-	_mag_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &_params->mag_p_noise)),
+        _gyro_noise(new control::BlockParamFloat(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise)),
+        _accel_noise(new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &_params->accel_noise)),
+        _gyro_bias_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise)),
+        _accel_bias_p_noise(new control::BlockParamFloat(this, "EKF2_ACC_B_NOISE", false, &_params->accel_bias_p_noise)),
+        _gyro_scale_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_S_NOISE", false, &_params->gyro_scale_p_noise)),
+        _mag_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_B_NOISE", false, &_params->mag_p_noise)),
 	_wind_vel_p_noise(new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise)),
 	_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)),
@@ -218,7 +219,8 @@ Ekf2::Ekf2():
 	_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)),
+        _mag_noise(new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &_params->mag_noise)),
+        _mag_declination_deg(new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &_params->mag_declination_deg)),
 	_heading_innov_gate(new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, &_params->heading_innov_gate)),
 	_mag_innov_gate(new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, &_params->mag_innov_gate)),
 	_gps_check_mask(new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, &_params->gps_check_mask)),
@@ -228,8 +230,7 @@ Ekf2::Ekf2():
 	_requiredNsats(new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &_params->req_nsats)),
 	_requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)),
 	_requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)),
-	_requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift))
-
+        _requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift))
 {
 
 }

src/modules/ekf2/ekf2_params.c

@@ -42,7 +42,7 @@
 #include <systemlib/param/param.h>
 
 /**
-* Magnetometer measurement delay
+* Magnetometer measurement delay relative to IMU measurements
 *
 * @group EKF2
 * @min 0
@@ -52,7 +52,7 @@
 PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0);
 
 /**
- * Barometer measurement delay
+ * Barometer measurement delay relative to IMU measurements
  *
  * @group EKF2
  * @min 0
@@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0);
 PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0);
 
 /**
- * GPS measurement delay
+ * GPS measurement delay relative to IMU measurements
  *
  * @group EKF2
  * @min 0
@@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0);
 PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 200);
 
 /**
- * Airspeed measurement delay
+ * Airspeed measurement delay relative to IMU measurements
  *
  * @group EKF2
  * @min 0
@@ -171,113 +171,124 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.3f);
 PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f);
 
 /**
- * Gyro noise.
+ * Rate gyro noise for covariance prediction.
  *
  * @group EKF2
  * @min 0.0001
- * @max 0.05
+ * @max 0.01
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(EKF2_G_NOISE, 0.001f);
+PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.0e-3f);
 
 /**
- * Process noise for delta velocity prediction.
+ * Accelerometer noise for covariance prediction.
  *
  * @group EKF2
  * @min 0.01
- * @max 1
- * @unit
+ * @max 1.0
+ * @unit m/s/s
  */
-PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.1f);
+PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.25f);
 
 /**
  * Process noise for delta angle bias prediction.
  *
  * @group EKF2
- * @min 0
+ * @min 0.0
  * @max 0.0001
- * @unit
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(EKF2_GB_NOISE, 1e-5f);
+PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 7.0e-5f);
 
 /**
  * Process noise for delta velocity z bias prediction.
  *
  * @group EKF2
- * @min 0.000001
+ * @min 0.0
  * @max 0.01
- * @unit
+ * @unit m/s/s
  */
-PARAM_DEFINE_FLOAT(EKF2_ACCB_NOISE, 1e-3f);
+PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 1.0e-4f);
 
 /**
- * Process noise for delta angle scale prediction.
+ * Process noise for delta angle scale factor prediction.
  *
  * @group EKF2
- * @min 0.000001
+ * @min 0.0
  * @max 0.01
- * @unit
+ * @unit None
  */
-PARAM_DEFINE_FLOAT(EKF2_GS_NOISE, 1e-4f);
+PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-3f);
 
 /**
- * Process noise for earth magnetic field and bias prediction.
+ * Process noise for sensor bias and earth magnetic field prediction.
  *
  * @group EKF2
- * @min 0.0001
+ * @min 0.0
  * @max 0.1
- * @unit
+ * @unit Gauss/s
  */
-PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 1e-2f);
+PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 2.5e-2f);
 
 /**
  * Process noise for wind velocity prediction.
  *
  * @group EKF2
- * @min 0.01
- * @max 1
- * @unit
+ * @min 0.0
+ * @max 1.0
+ * @unit m/s/s
  */
-PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 0.05f);
+PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 1.0e-1f);
 
 /**
- * Measurement noise for gps velocity.
+ * Measurement noise for gps horizontal velocity.
  *
  * @group EKF2
- * @min 0.001
- * @max 0.5
- * @unit
+ * @min 0.01
+ * @max 5.0
+ * @unit m/s
  */
-PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.05f);
+PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.5f);
 
 /**
  * Measurement noise for gps position.
  *
  * @group EKF2
  * @min 0.01
- * @max 5
- * @unit
+ * @max 10.0
+ * @unit m
  */
 PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 1.0f);
 
 /**
- * Measurement noise for barometer.
+ * Measurement noise for barometric altitude.
+ *
+ * @group EKF2
+ * @min 0.01
+ * @max 15.0
+ * @unit m
+ */
+PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.0f);
+
+/**
+ * Measurement noise for magnetic heading fusion.
+ *
+ * @group EKF2
+ * @min 0.01
+ * @max 1.0
+ * @unit rad
+ */
+PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 0.17f);
+
+/**
+ * Measurement noise for magnetometer 3-axis fusion.
  *
  * @group EKF2
  * @min 0.001
- * @max 1
- * @unit
+ * @max 1.0
+ * @unit Gauss
  */
-PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 0.1f);
-
-/**
- * Measurement noise for mag heading fusion.
- *
- * @group EKF2
- * @min 0.0001
- * @max 0.1
- * @unit
- */
-PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3e-2f);
+PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f);
 
 /**
  * Magnetic declination
@@ -288,36 +299,46 @@ PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3e-2f);
 PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0);
 
 /**
- * Gate size for magnetic heading fusion (standard deviations)
+ * Gate size for magnetic heading fusion
  *
  * @group EKF2
+ * @min 1.0
+ * @unit SD
  */
 PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f);
 
 /**
- * Gate size for magnetometer XYZ component fusion (standard deviations)
+ * Gate size for magnetometer XYZ component fusion
  *
  * @group EKF2
+ * @min 1.0
+ * @unit SD
  */
 PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f);
 
 /**
- * Gate size for barometric height fusion (standard deviations)
+ * Gate size for barometric height fusion
  *
  * @group EKF2
+ * @min 1.0
+ * @unit SD
  */
-PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f);
+PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 3.0f);
 
 /**
- * Gate size for GPS horizontal position fusion (standard deviations)
+ * Gate size for GPS horizontal position fusion
  *
  * @group EKF2
+ * @min 1.0
+ * @unit SD
  */
-PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f);
+PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f);
 
 /**
- * Gate size for GPS velocity fusion (standard deviations)
+ * Gate size for GPS velocity fusion
  *
  * @group EKF2
+ * @min 1.0
+ * @unit SD
  */
 PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f);