ekf2: save full covariance sub-matrix for mag I/B

src/modules/ekf2/EKF/covariance.cpp

@@ -1021,7 +1021,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 	// magnetic field states
 	if (!_control_status.flags.mag_3D) {
-		zeroMagCov();
+		P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
+		P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
 
 	} else {
 		// constrain variances
@@ -1101,29 +1102,13 @@ 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();
+	_mag_decl_cov_reset = false;
 
 	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()
-{
-	zeroMagCov();
-	_mag_decl_cov_reset = false;
-}
-
-void Ekf::zeroMagCov()
-{
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
+	// save covariance data for re-use when auto-switching between heading and 3-axis fusion
+	saveMagCovData();
 }
 
 void Ekf::resetZDeltaAngBiasCov()

src/modules/ekf2/EKF/ekf.h

@@ -255,7 +255,7 @@ public:
 			return Vector3f{P(19, 19), P(20, 20), P(21, 21)};
 		}
 
-		return _saved_mag_bf_variance;
+		return _saved_mag_bf_covmat.diag();
 	}
 
 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
@@ -529,9 +529,8 @@ private:
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
-	Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
-	Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
-	float _saved_mag_ef_d_variance{};   ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
+	Matrix3f _saved_mag_bf_covmat{}; ///< magnetometer bias state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
+	Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
 
 	float _mag_heading_last_declination{NAN};
 
@@ -963,10 +962,7 @@ private:
 	void increaseQuatYawErrVariance(float yaw_variance);
 
 	// load and save mag field state covariance data for re-use
-	void loadMagCovData();
 	void saveMagCovData();
-	void clearMagCov();
-	void zeroMagCov();
 
 	void resetZDeltaAngBiasCov();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -865,7 +865,7 @@ void Ekf::resetMagBias()
 	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
 
 	// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
-	_saved_mag_bf_variance.zero();
+	_saved_mag_bf_covmat.zero();
 
 	_mag_counter = 0;
 }
@@ -1198,13 +1198,18 @@ void Ekf::stopMagFusion()
 {
 	stopMag3DFusion();
 	stopMagHdgFusion();
-	clearMagCov();
+
+	P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
+	P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
+
+	_mag_decl_cov_reset = false;
 }
 
 void Ekf::stopMag3DFusion()
 {
 	// save covariance data for re-use if currently doing 3-axis fusion
 	if (_control_status.flags.mag_3D) {
+
 		saveMagCovData();
 
 		_control_status.flags.mag_3D = false;
@@ -1250,8 +1255,12 @@ void Ekf::startMag3DFusion()
 
 		_yaw_test_ratio = 0.0f;
 
-		zeroMagCov();
-		loadMagCovData();
+		// re-instate the NE axis covariance sub-matrix
+		P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
+
+		// re-instate variances for the XYZ body axis field
+		P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
+
 		_control_status.flags.mag_3D = true;
 	}
 }
@@ -1497,33 +1506,11 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 // save covariance data for re-use when auto-switching between heading and 3-axis fusion
 void Ekf::saveMagCovData()
 {
-	// save variances for XYZ body axis field
-	_saved_mag_bf_variance(0) = P(19, 19);
-	_saved_mag_bf_variance(1) = P(20, 20);
-	_saved_mag_bf_variance(2) = P(21, 21);
-
 	// save the NE axis covariance sub-matrix
-	_saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16);
+	_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
 
-	// save variance for the D earth axis
-	_saved_mag_ef_d_variance = P(18, 18);
-}
-
-void Ekf::loadMagCovData()
-{
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
-
-	// re-instate variances for the XYZ body axis field
-	P(19, 19) = _saved_mag_bf_variance(0);
-	P(20, 20) = _saved_mag_bf_variance(1);
-	P(21, 21) = _saved_mag_bf_variance(2);
-
-	// re-instate the NE axis covariance sub-matrix
-	P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat;
-
-	// re-instate the D earth axis variance
-	P(18, 18) = _saved_mag_ef_d_variance;
+	// save variances for XYZ body axis field
+	_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
 }
 
 void Ekf::startAirspeedFusion()