EKF: Don't discard declination certainty information when resuming 3-axis fusion.

In the case where the EKF is switching between 3-axis and heading fusion, off-diagonal elements containing the correlation between N,E  components of the earth field were being lost on each switch event. These elements contained information about the declination uncertainty and should be preserved.

EKF/control.cpp

@@ -1463,9 +1463,16 @@ void Ekf::controlMagFusion()
 						zeroRows(P, 16, 21);
 						zeroCols(P, 16, 21);
 
-						// re-instate the last used variances
-						for (uint8_t index = 0; index <= 5; index ++) {
-							P[index + 16][index + 16] = _saved_mag_variance[index];
+						// re-instate variances for the D earth axis and XYZ body axis field
+						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 ++) {
+								P[row + 16][col + 16] = _saved_mag_ef_covmat[row][col];
+							}
 						}
 					}
 				}
@@ -1475,10 +1482,18 @@ void Ekf::controlMagFusion()
 				_control_status.flags.mag_hdg = !_control_status.flags.mag_3D;
 
 			} else {
-				// save magnetic field state variances for next time
+				// save magnetic field state covariance data for next time
 				if (_control_status.flags.mag_3D) {
-					for (uint8_t index = 0; index <= 5; index ++) {
-						_saved_mag_variance[index] = P[index + 16][index + 16];
+					// save variances for the D earth axis and XYZ body axis field
+					for (uint8_t index = 0; index <= 3; index ++) {
+						_saved_mag_bf_variance[index] = P[index + 18][index + 18];
+					}
+
+					// save the NE axis covariance sub-matrix
+					for (uint8_t row = 0; row <= 1; row ++) {
+						for (uint8_t col = 0; col <= 1; col ++) {
+							_saved_mag_ef_covmat[row][col] = P[row + 16][col + 16];
+						}
 					}
 
 					_control_status.flags.mag_3D = false;

EKF/ekf.h

@@ -414,7 +414,8 @@ private:
 	bool _flt_mag_align_converging{false};	///< true when the in-flight mag field post alignment convergence is being performd
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_movement{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
-	float _saved_mag_variance[6] {};	///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
+	float _saved_mag_bf_variance[4] {};	///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
+	float _saved_mag_ef_covmat[2][2] {};    ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
 	bool _velpos_reset_request{false};	///< true when a large yaw error has been fixed and a velocity and position state reset is required
 
 	gps_check_fail_status_u _gps_check_fail_status{};