EKF: Save mag field covariance data before reset

EKF/control.cpp

@@ -239,12 +239,17 @@ void Ekf::controlExternalVisionFusion()
 				// flag the yaw as aligned
 				_control_status.flags.yaw_align = true;
 
-				// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
+				// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
 				_control_status.flags.ev_yaw = true;
 				_control_status.flags.mag_hdg = false;
-				_control_status.flags.mag_3D = false;
 				_control_status.flags.mag_dec = false;
 
+				// save covariance data for re-use if currently doing 3-axis fusion
+				if (_control_status.flags.mag_3D) {
+					save_mag_cov_data();
+					_control_status.flags.mag_3D = false;
+				}
+
 				ECL_INFO("EKF commencing external vision yaw fusion");
 			}
 		}
@@ -530,9 +535,14 @@ void Ekf::controlGpsFusion()
 				_control_status.flags.gps_yaw = true;
 				_control_status.flags.ev_yaw = false;
 				_control_status.flags.mag_hdg = false;
-				_control_status.flags.mag_3D = false;
 				_control_status.flags.mag_dec = false;
 
+				// save covariance data for re-use if currently doing 3-axis fusion
+				if (_control_status.flags.mag_3D) {
+					save_mag_cov_data();
+					_control_status.flags.mag_3D = false;
+				}
+
 				ECL_INFO("EKF commencing GPS yaw fusion");
 			}
 		}
@@ -1341,11 +1351,17 @@ void Ekf::controlDragFusion()
 void Ekf::controlMagFusion()
 {
 	if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
+
 		// do not use the magnetomer and deactivate magnetic field states
+		// save covariance data for re-use if currently doing 3-axis fusion
+		if (_control_status.flags.mag_3D) {
+			save_mag_cov_data();
+			_control_status.flags.mag_3D = false;
+		}
 		zeroRows(P, 16, 21);
 		zeroCols(P, 16, 21);
 		_control_status.flags.mag_hdg = false;
-		_control_status.flags.mag_3D = false;
+
 		return;
 	}
 
@@ -1462,7 +1478,6 @@ void Ekf::controlMagFusion()
 						for (uint8_t index = 0; index <= 3; index ++) {
 							P[index + 18][index + 18] = _saved_mag_bf_variance[index];
 						}
-
 						// re-instate the NE axis covariance sub-matrix
 						for (uint8_t row = 0; row <= 1; row ++) {
 							for (uint8_t col = 0; col <= 1; col ++) {
@@ -1477,7 +1492,7 @@ void Ekf::controlMagFusion()
 				_control_status.flags.mag_hdg = !_control_status.flags.mag_3D;
 
 			} else {
-				// save magnetic field state covariance data for next time
+				// save covariance data for re-use if currently doing 3-axis fusion
 				if (_control_status.flags.mag_3D) {
 					save_mag_cov_data();
 					_control_status.flags.mag_3D = false;
@@ -1510,17 +1525,24 @@ void Ekf::controlMagFusion()
 					P[index + 16][index + 16] = sq(_params.mag_noise);
 				}
 
-				// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
-				fuseDeclination(0.02f);
+				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) {
 			// always use heading fusion
 			_control_status.flags.mag_hdg = true;
-			_control_status.flags.mag_3D = false;
+
+			// save covariance data for re-use if currently doing 3-axis fusion
+			if (_control_status.flags.mag_3D) {
+				save_mag_cov_data();
+				_control_status.flags.mag_3D = false;
+			}
 
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
 			// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
@@ -1536,7 +1558,13 @@ void Ekf::controlMagFusion()
 		} else {
 			// do no magnetometer fusion at all
 			_control_status.flags.mag_hdg = false;
-			_control_status.flags.mag_3D = false;
+
+			// save covariance data for re-use if currently doing 3-axis fusion
+			if (_control_status.flags.mag_3D) {
+				save_mag_cov_data();
+				_control_status.flags.mag_3D = false;
+			}
+
 		}
 
 		// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift

EKF/ekf_helper.cpp

@@ -487,15 +487,17 @@ bool Ekf::realignYawGPS()
 			zeroRows(P, 16, 21);
 			zeroCols(P, 16, 21);
 
-			for (uint8_t index = 16; index <= 21; index ++) {
-				P[index][index] = sq(_params.mag_noise);
-			}
+			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);
+				// 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();
+			}
 
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -528,15 +530,17 @@ bool Ekf::realignYawGPS()
 			zeroRows(P, 16, 21);
 			zeroCols(P, 16, 21);
 
-			for (uint8_t index = 16; index <= 21; index ++) {
-				P[index][index] = sq(_params.mag_noise);
-			}
+			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);
+				// 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();
+			}
 
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@@ -561,10 +565,15 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 
 	if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
 		// do not use the magnetomer and deactivate magnetic field states
+		// save covariance data for re-use if currently doing 3-axis fusion
+		if (_control_status.flags.mag_3D) {
+			save_mag_cov_data();
+			_control_status.flags.mag_3D = false;
+		}
 		zeroRows(P, 16, 21);
 		zeroCols(P, 16, 21);
 		_control_status.flags.mag_hdg = false;
-		_control_status.flags.mag_3D = false;
+
 		return false;
 	}
 
@@ -693,15 +702,17 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	zeroRows(P, 16, 21);
 	zeroCols(P, 16, 21);
 
-	for (uint8_t index = 16; index <= 21; index ++) {
-		P[index][index] = sq(_params.mag_noise);
-	}
+	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);
+		// 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();
+	}
 
 	// record the time for the magnetic field alignment event
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;

EKF/mag_fusion.cpp

@@ -916,7 +916,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 			}
 		}
 
-		// correct the covariance marix for gross errors
+		// correct the covariance matrix for gross errors
 		fixCovarianceErrors();
 
 		// apply the state corrections