diff --git a/EKF/control.cpp b/EKF/control.cpp
index baf2469293..e7c05d7a35 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -1484,18 +1484,7 @@ void Ekf::controlMagFusion()
 			} else {
 				// save magnetic field state covariance data for next time
 				if (_control_status.flags.mag_3D) {
-					// 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];
-						}
-					}
-
+					save_mag_cov_data();
 					_control_status.flags.mag_3D = false;
 				}
 
@@ -1528,6 +1517,9 @@ void Ekf::controlMagFusion()
 
 				// 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();
 			}
 
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index fc365f408e..f762cd8d20 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -892,6 +892,9 @@ void Ekf::resetMagCovariance()
 
 	// 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();
 }
 
 void Ekf::resetWindCovariance()
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 40abae8440..8560051fb8 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -674,6 +674,9 @@ private:
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
 
+	// save mag field state covariance data for re-use
+	void save_mag_cov_data();
+
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatStates();
 
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index bc59009535..3c312e10a5 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -494,6 +494,9 @@ bool Ekf::realignYawGPS()
 			// 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();
+
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
@@ -532,6 +535,9 @@ bool Ekf::realignYawGPS()
 			// 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();
+
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
@@ -694,6 +700,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	// 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();
+
 	// record the time for the magnetic field alignment event
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
@@ -1708,3 +1717,19 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 	P[3][1] -= yaw_variance*SQ[1]*SQ[3];
 	P[3][2] -= yaw_variance*SQ[0]*SQ[3];
 }
+
+// save covariance data for re-use when auto-switching between heading and 3-axis fusion
+void Ekf::save_mag_cov_data()
+{
+	// 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];
+		}
+	}
+}