ekf2: centralize mag covariance reset (#693)

* ekf2: centralize mag covariance reset. A complete and clean reset of the
mag states covariances is now performed through the "resetMagCov"
function only. This avoid having slight differences of implementations
across the code.

* Ekf: reset quat cov with initial uncertainty instead of zero

EKF/covariance.cpp

@@ -57,11 +57,7 @@ void Ekf::initialiseCovariance()
 
 	const float dt = FILTER_UPDATE_PERIOD_S;
 
-	// define the initial angle uncertainty as variances for a rotation vector
+	resetQuatCov();
-	Vector3f rot_vec_var;
-	rot_vec_var.setAll(sq(_params.initial_tilt_err));
-
-	initialiseQuatCovariances(rot_vec_var);
 
 	// velocity
 	P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f));
@@ -97,14 +93,7 @@ void Ekf::initialiseCovariance()
 	// record IMU bias state covariance reset time - used to prevent resets being performed too often
 	_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
 
-	// earth frame and body frame magnetic field
+	resetMagCov();
-	// set to observation variance
-	for (uint8_t index = 16; index <= 21; index ++) {
-		P(index,index) = sq(_params.mag_noise);
-	}
-
-	// save covariance data for re-use when auto-switching between heading and 3-axis fusion
-	saveMagCovData();
 
 	// wind
 	P(22,22) = sq(_params.initial_wind_uncertainty);
@@ -869,17 +858,42 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 void Ekf::resetMagRelatedCovariances()
 {
-	// set the quaternion covariance terms to zero
+	resetQuatCov();
+	resetMagCov();
+}
+
+void Ekf::resetQuatCov(){
+	zeroQuatCov();
+
+	// define the initial angle uncertainty as variances for a rotation vector
+	Vector3f rot_vec_var;
+	rot_vec_var.setAll(sq(_params.initial_tilt_err));
+
+	initialiseQuatCovariances(rot_vec_var);
+}
+
+void Ekf::zeroQuatCov()
+{
 	P.uncorrelateCovarianceSetVariance<2>(0, 0.0f);
 	P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);
+}
+
+void Ekf::resetMagCov()
+{
+	// reset the corresponding rows and columns in the covariance matrix and
+	// set the variances on the magnetic field states to the measurement variance
+	clearMagCov();
 
-	// reset the field state variance to the observation variance
 	P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise));
 	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
 
+	if (!_control_status.flags.mag_3D) {
 		// save covariance data for re-use when auto-switching between heading and 3-axis fusion
+		// if already in 3-axis fusion mode, the covariances are automatically saved when switching out
+		// of this mode
 		saveMagCovData();
 	}
+}
 
 void Ekf::clearMagCov()
 {

EKF/ekf.h

@@ -760,9 +760,13 @@ private:
 	// do not call before quaternion states are initialised
 	void initialiseQuatCovariances(Vector3f &rot_vec_var);
 
-	// perform a limited reset of the magnetic field state covariances
+	// perform a limited reset of the magnetic field related state covariances
 	void resetMagRelatedCovariances();
 
+	void resetQuatCov();
+	void zeroQuatCov();
+	void resetMagCov();
+
 	// perform a limited reset of the wind state covariances
 	void resetWindCovariance();
 

EKF/ekf_helper.cpp

@@ -459,17 +459,7 @@ bool Ekf::realignYawGPS()
 			_R_to_earth = Dcmf(_state.quat_nominal);
 			_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
 
-			// reset the corresponding rows and columns in the covariance matrix and set the
+			resetMagCov();
-			// variances on the magnetic field states to the measurement variance
-			clearMagCov();
-			if (_control_status.flags.mag_3D) {
-				for (uint8_t index = 16; index <= 21; index ++) {
-					P(index,index) = sq(_params.mag_noise);
-				}
-
-				// save covariance data for re-use when auto-switching between heading and 3-axis fusion
-				saveMagCovData();
-			}
 
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -485,17 +475,7 @@ bool Ekf::realignYawGPS()
 			// calculate initial earth magnetic field states
 			_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
 
-			// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
+			resetMagCov();
-			clearMagCov();
-
-			if (_control_status.flags.mag_3D) {
-				for (uint8_t index = 16; index <= 21; index ++) {
-					P(index,index) = sq(_params.mag_noise);
-				}
-
-				// save covariance data for re-use when auto-switching between heading and 3-axis fusion
-				saveMagCovData();
-			}
 
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -579,17 +559,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	// set the earth magnetic field states using the updated rotation
 	_state.mag_I = _R_to_earth * mag_init;
 
-	// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
+	resetMagCov();
-	clearMagCov();
-
-	if (_control_status.flags.mag_3D) {
-		for (uint8_t index = 16; index <= 21; index ++) {
-			P(index,index) = sq(_params.mag_noise);
-		}
-
-		// save covariance data for re-use when auto-switching between heading and 3-axis fusion
-		saveMagCovData();
-	}
 
 	// record the time for the magnetic field alignment event
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;