diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp
index c87022c511..6274fe7756 100644
--- a/src/modules/ekf2/EKF/mag_fusion.cpp
+++ b/src/modules/ekf2/EKF/mag_fusion.cpp
@@ -57,7 +57,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 	const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
 
 	// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
-	const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
 	Vector3f mag_innov;
 	Vector3f innov_var;
 
@@ -66,60 +65,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 	const VectorState state_vector = _state.vector();
 	sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
 
-	innov_var.copyTo(aid_src_mag.innovation_variance);
-
-	if (aid_src_mag.innovation_variance[0] < R_MAG) {
-		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-		_fault_status.flags.bad_mag_x = true;
-
-		// we need to re-initialise covariances and abort this fusion step
-		if (update_all_states) {
-			resetQuatCov(_params.mag_heading_noise);
-		}
-
-		resetMagCov();
-
-		ECL_ERR("magX %s", numerical_error_covariance_reset_string);
-		return false;
-	}
-
-	_fault_status.flags.bad_mag_x = false;
-
-	// check innovation variances for being badly conditioned
-	if (aid_src_mag.innovation_variance[1] < R_MAG) {
-		// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
-		_fault_status.flags.bad_mag_y = true;
-
-		// we need to re-initialise covariances and abort this fusion step
-		if (update_all_states) {
-			resetQuatCov(_params.mag_heading_noise);
-		}
-
-		resetMagCov();
-
-		ECL_ERR("magY %s", numerical_error_covariance_reset_string);
-		return false;
-	}
-
-	_fault_status.flags.bad_mag_y = false;
-
-	if (aid_src_mag.innovation_variance[2] < R_MAG) {
-		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-		_fault_status.flags.bad_mag_z = true;
-
-		// we need to re-initialise covariances and abort this fusion step
-		if (update_all_states) {
-			resetQuatCov(_params.mag_heading_noise);
-		}
-
-		resetMagCov();
-
-		ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
-		return false;
-	}
-
-	_fault_status.flags.bad_mag_z = false;
-
 	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
 	if (_control_status.flags.synthetic_mag_z) {
 		mag_innov(2) = 0.0f;
@@ -129,21 +74,37 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		aid_src_mag.observation[i] = mag(i) - _state.mag_B(i);
 		aid_src_mag.observation_variance[i] = R_MAG;
 		aid_src_mag.innovation[i] = mag_innov(i);
-	}
-
-	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
-	if (_control_status.flags.synthetic_mag_z) {
-		aid_src_mag.innovation[2] = 0.0f;
+		aid_src_mag.innovation_variance[i] = innov_var(i);
 	}
 
 	const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
 	setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
 
+	// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+	_fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]);
+	_fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]);
+	_fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]);
+
 	// Perform an innovation consistency check and report the result
 	_innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f);
 	_innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f);
 	_innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f);
 
+	const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
+
+	// check innovation variances for being badly conditioned
+	if (innov_var.min() < R_MAG) {
+		// we need to re-initialise covariances and abort this fusion step
+		if (update_all_states) {
+			resetQuatCov(_params.mag_heading_noise);
+		}
+
+		resetMagCov();
+
+		ECL_ERR("mag %s", numerical_error_covariance_reset_string);
+		return false;
+	}
+
 	// if any axis fails, abort the mag fusion
 	if (aid_src_mag.innovation_rejected) {
 		return false;