EKF: Rework use of fuseDeclination()

Simplify calling so that it is only called in two ways:
1) Immediately before 3-axis mag fusion if not called since the last earth field covariance reset so that the earth field declination information can be formed.
2) Immediately after 3-axis mag fusion otherwise.

EKF/control.cpp

@@ -1365,6 +1365,7 @@ void Ekf::controlMagFusion()
 		}
 		zeroRows(P, 16, 21);
 		zeroCols(P, 16, 21);
+		_mag_decl_cov_reset = false;
 		_control_status.flags.mag_hdg = false;
 
 		return;
@@ -1519,24 +1520,20 @@ void Ekf::controlMagFusion()
 			// before they are used to constrain heading drift
 			_flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6);
 
-			if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) {
+			if (_control_status.flags.mag_3D && _control_status_prev.flags.update_mag_states_only && !_control_status.flags.update_mag_states_only) {
 				// When re-commencing use of magnetometer to correct vehicle states
 				// set the field state variance to the observation variance and zero
 				// the covariance terms to allow the field states re-learn rapidly
 				zeroRows(P, 16, 21);
 				zeroCols(P, 16, 21);
+				_mag_decl_cov_reset = false;
 
 				for (uint8_t index = 0; index <= 5; index ++) {
 					P[index + 16][index + 16] = sq(_params.mag_noise);
 				}
 
-				if (_control_status.flags.mag_3D) {
-					// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
-					fuseDeclination(0.02f);
-
-					// save covariance data for re-use when auto-switching between heading and 3-axis fusion
-					save_mag_cov_data();
-				}
+				// save covariance data for re-use when auto-switching between heading and 3-axis fusion
+				save_mag_cov_data();
 			}
 
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
@@ -1602,10 +1599,22 @@ void Ekf::controlMagFusion()
 
 		// fuse magnetometer data using the selected methods
 		if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
-			fuseMag();
-
-			if (_control_status.flags.mag_dec) {
-				fuseDeclination(0.5f);
+			if (!_mag_decl_cov_reset) {
+				// After any magnetic field covariance reset event the earth field state
+				// covariances need to be corrected to incorporate knowedge of the declination
+				// before fusing magnetomer data to prevent rapid rotation of the earth field
+				// states for the first few observations.
+				fuseDeclination(0.02f);
+				_mag_decl_cov_reset = true;
+				fuseMag();
+			} else {
+				// The normal sequence is to fuse the magnetmer data first before fusing
+				// declination angle at a higher uncertainty to allow some learning of
+				// declination angle over time.
+				fuseMag();
+				if (_control_status.flags.mag_dec) {
+					fuseDeclination(0.5f);
+				}
 			}
 
 		} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {

EKF/covariance.cpp

@@ -847,6 +847,7 @@ void Ekf::fixCovarianceErrors()
 	if (!_control_status.flags.mag_3D) {
 		zeroRows(P, 16, 21);
 		zeroCols(P, 16, 21);
+		_mag_decl_cov_reset = false;
 
 	} else {
 		// constrain variances
@@ -887,15 +888,13 @@ void Ekf::resetMagCovariance()
 	// set the magnetic field covariance terms to zero
 	zeroRows(P, 16, 21);
 	zeroCols(P, 16, 21);
+	_mag_decl_cov_reset = false;
 
 	// set the field state variance to the observation variance
 	for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) {
 		P[rc_index][rc_index] = sq(_params.mag_noise);
 	}
 
-	// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
-	fuseDeclination(0.02f);
-
 	// save covariance data for re-use when auto-switching between heading and 3-axis fusion
 	save_mag_cov_data();
 }

EKF/ekf.h

@@ -340,6 +340,7 @@ private:
 	float _last_static_yaw{0.0f};		///< last yaw angle recorded when on ground motion checks were passing (rad)
 	bool _vehicle_at_rest_prev{false};	///< true when the vehicle was at rest the previous time the status was checked
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetomer data has been requested
+	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
 
 	float P[_k_num_states][_k_num_states] {};	///< state covariance matrix
 

EKF/ekf_helper.cpp

@@ -486,15 +486,13 @@ bool Ekf::realignYawGPS()
 			// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
 			zeroRows(P, 16, 21);
 			zeroCols(P, 16, 21);
+			_mag_decl_cov_reset = false;
 
 			if (_control_status.flags.mag_3D) {
 				for (uint8_t index = 16; index <= 21; index ++) {
 					P[index][index] = sq(_params.mag_noise);
 				}
 
-				// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
-				fuseDeclination(0.02f);
-
 				// save covariance data for re-use when auto-switching between heading and 3-axis fusion
 				save_mag_cov_data();
 			}
@@ -529,15 +527,13 @@ bool Ekf::realignYawGPS()
 			// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
 			zeroRows(P, 16, 21);
 			zeroCols(P, 16, 21);
+			_mag_decl_cov_reset = false;
 
 			if (_control_status.flags.mag_3D) {
 				for (uint8_t index = 16; index <= 21; index ++) {
 					P[index][index] = sq(_params.mag_noise);
 				}
 
-				// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
-				fuseDeclination(0.02f);
-
 				// save covariance data for re-use when auto-switching between heading and 3-axis fusion
 				save_mag_cov_data();
 			}
@@ -572,6 +568,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		}
 		zeroRows(P, 16, 21);
 		zeroCols(P, 16, 21);
+		_mag_decl_cov_reset = false;
 		_control_status.flags.mag_hdg = false;
 
 		return false;
@@ -701,15 +698,13 @@ bool Ekf::resetMagHeading(Vector3f &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
 	zeroRows(P, 16, 21);
 	zeroCols(P, 16, 21);
+	_mag_decl_cov_reset = false;
 
 	if (_control_status.flags.mag_3D) {
 		for (uint8_t index = 16; index <= 21; index ++) {
 			P[index][index] = sq(_params.mag_noise);
 		}
 
-		// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
-		fuseDeclination(0.02f);
-
 		// save covariance data for re-use when auto-switching between heading and 3-axis fusion
 		save_mag_cov_data();
 	}