EKF: 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).
Scale vertical GPS noise to allow for average EPV to EPH ratio.
Update documentation and display names for consistency.

EKF/common.h

@@ -50,7 +50,7 @@ struct gps_message {
 	float eph;                  // GPS horizontal position accuracy in m
 	float epv;                  // GPS vertical position accuracy in m
 	float sacc;                 // GPS speed accuracy in m/s
-	uint64_t time_usec_vel; 	// Timestamp for velocity informations
+        uint64_t time_usec_vel;     // Timestamp for velocity informations
 	float vel_m_s;              // GPS ground speed (m/s)
 	float vel_ned[3];           // GPS ground speed NED
 	bool vel_ned_valid;         // GPS ground speed is valid
@@ -112,33 +112,34 @@ struct flowSample {
 };
 
 struct parameters {
-	float mag_delay_ms = 0.0f;
-	float baro_delay_ms = 0.0f;
-	float gps_delay_ms = 200.0f;
-	float airspeed_delay_ms = 200.0f;
+        float mag_delay_ms = 0.0f;          // magnetometer measurement delay relative to the IMU
+        float baro_delay_ms = 0.0f;         // barometer height measurement delay relative to the IMU
+        float gps_delay_ms = 200.0f;        // GPS measurement delay relative to the IMU
+        float airspeed_delay_ms = 200.0f;   // airspeed measurement delay relative to the IMU
 
 	// input noise
-	float gyro_noise = 0.001f;
-	float accel_noise = 0.1f;
+        float gyro_noise = 1.0e-3f;         // IMU angular rate noise used for covariance prediction
+        float accel_noise = 2.5e-1f;        // IMU acceleration noise use for covariance prediction
 
 	// process noise
-	float gyro_bias_p_noise = 1e-5f;
-	float accel_bias_p_noise = 1e-3f;
-	float gyro_scale_p_noise = 1e-4f;
-	float mag_p_noise = 1e-2f;
-	float wind_vel_p_noise = 0.05f;
-
-	float gps_vel_noise = 0.05f;
-	float gps_pos_noise = 1.0f;
-	float baro_noise = 0.1f;
-	float baro_innov_gate = 5.0f;       // barometric height innovation consistency gate size in standard deviations
-	float posNE_innov_gate = 5.0f;      // GPS horizontal position innovation consistency gate size in standard deviations
+        float gyro_bias_p_noise = 7.0e-5f;  // process noise for IMU delta angle bias prediction
+        float accel_bias_p_noise = 1.0e-4f; // process noise for IMU delta velocity bias prediction
+        float gyro_scale_p_noise = 3.0e-3f; // process noise for gyro scale factor prediction
+        float mag_p_noise = 2.5e-2f;        // process noise for magnetic field prediction
+        float wind_vel_p_noise = 1.0e-1f;   // process noise for wind velocity prediction
+
+        float gps_vel_noise = 5.0e-1f;      // observation noise for gps velocity fusion
+        float gps_pos_noise = 1.0f;         // observation noise for gps position fusion
+        float baro_noise = 3.0f;            // observation noise for barometric height fusion
+        float baro_innov_gate = 3.0f;       // barometric height innovation consistency gate size in standard deviations
+        float posNE_innov_gate = 3.0f;      // GPS horizontal position innovation consistency gate size in standard deviations
 	float vel_innov_gate = 3.0f;        // GPS velocity innovation consistency gate size in standard deviations
 
-	float mag_heading_noise = 3e-2f;	// measurement noise used for simple heading fusion
-	float mag_declination_deg = 0.0f;	// magnetic declination in degrees
-	float heading_innov_gate = 3.0f;	// heading fusion innovation consistency gate size in standard deviations
-	float mag_innov_gate = 3.0f;        // magnetometer fusion innovation consistency gate size in standard deviations
+        float mag_heading_noise = 1.7e-1f;  // measurement noise used for simple heading fusion
+        float mag_noise = 5.0e-2f;          // measurement noise used for 3-axis magnetoemeter fusion
+        float mag_declination_deg = 0.0f;   // magnetic declination in degrees
+        float heading_innov_gate = 3.0f;    // heading fusion innovation consistency gate size in standard deviations
+        float mag_innov_gate = 3.0f;        // magnetometer fusion innovation consistency gate size in standard deviations
 
 	// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
 	// good enough to set a local origin and commence aiding

EKF/covariance.cpp

@@ -130,12 +130,12 @@ void Ekf::predictCovariance()
 	// compute process noise
 	float process_noise[_k_num_states] = {};
 
-	float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
-	float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 1e-6f, 1e-2f);
-	float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 1e-6f, 1e-2f);
-	float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f);
-	float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f);
-	float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.01f, 1.0f);
+    float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
+    float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f);
+    float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f);
+    float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
+    float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
+    float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
 
 	for (unsigned i = 0; i < 9; i++) {
 		process_noise[i] = 0.0f;

EKF/gps_checks.cpp

@@ -84,7 +84,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
  * Return true if the GPS solution quality is adequate to set an origin for the EKF
  * and start GPS aiding.
  * All activated checks must pass for 10 seconds.
- * Checks are activated using the EKF2_GPS_CHECKS bitmask parameter
+ * Checks are activated using the EKF2_GPS_CHECK bitmask parameter
  * Checks are adjusted using the EKF2_REQ_* parameters
 */
 bool Ekf::gps_is_good(struct gps_message *gps)

EKF/mag_fusion.cpp

@@ -55,7 +55,8 @@ void Ekf::fuseMag()
 	float magD = _state.mag_I(2);
 
 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-	float R_MAG = 1e-3f;
+    float R_MAG = fmaxf(_params.mag_noise, 1.0e-3f);
+    R_MAG = R_MAG*R_MAG;
 
 	// intermediate variables from algebraic optimisation
 	float SH_MAG[9];
@@ -471,7 +472,8 @@ void Ekf::fuseHeading()
 	float magY = _mag_sample_delayed.mag(1);
 	float magZ = _mag_sample_delayed.mag(2);
 
-	float R_mag = _params.mag_heading_noise;
+    float R_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f);
+    R_mag = R_mag*R_mag;
 
 	float t2 = q0 * q0;
 	float t3 = q1 * q1;

EKF/vel_pos_fusion.cpp

@@ -54,8 +54,8 @@ void Ekf::fuseVelPosHeight()
 		fuse_map[0] = fuse_map[1] = true;
 		_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
 		_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
-		R[0] = _params.gps_vel_noise;
-		R[1] = _params.gps_vel_noise;
+        R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
+        R[1] = R[0];
         gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
         gate_size[1] = gate_size[0];
     }
@@ -63,7 +63,7 @@ void Ekf::fuseVelPosHeight()
 	if (_fuse_vert_vel) {
 		fuse_map[2] = true;
 		_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
-		R[2] = _params.gps_vel_noise;
+        R[2] = 1.5f * fmaxf(_params.gps_vel_noise, 0.01f);
         gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
     }
 
@@ -71,8 +71,8 @@ void Ekf::fuseVelPosHeight()
 		fuse_map[3] = fuse_map[4] = true;
 		_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
 		_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
-		R[3] = _params.gps_pos_noise;
-		R[4] = _params.gps_pos_noise;
+        R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
+        R[4] = R[3];
         gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
         gate_size[4] = gate_size[3];
 	}
@@ -80,7 +80,7 @@ void Ekf::fuseVelPosHeight()
 	if (_fuse_height) {
 		fuse_map[5] = true;
 		_vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt);		// baro measurement has inversed z axis
-		R[5] = _params.baro_noise;
+        R[5] = fmaxf(_params.baro_noise, 0.01f);
         gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
     }