ekf2: estimator aid source status (mag heading, mag 3d)

msg/estimator_aid_source_1d.msg

@@ -20,3 +20,4 @@ bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_source_1d
 # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt
+# TOPICS estimator_aid_src_mag_heading

msg/estimator_aid_source_3d.msg

@@ -20,3 +20,4 @@ bool[3] fused                # true if the sample was successfully fused
 
 # TOPICS estimator_aid_source_3d
 # TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
+# TOPICS estimator_aid_src_mag

msg/estimator_status_flags.msg

@@ -65,9 +65,6 @@ bool reject_hor_vel                       #  0 - true if horizontal velocity obs
 bool reject_ver_vel                       #  1 - true if vertical velocity observations have been rejected
 bool reject_hor_pos                       #  2 - true if horizontal position observations have been rejected
 bool reject_ver_pos                       #  3 - true if vertical position observations have been rejected
-bool reject_mag_x                         #  4 - true if the X magnetometer observation has been rejected
-bool reject_mag_y                         #  5 - true if the Y magnetometer observation has been rejected
-bool reject_mag_z                         #  6 - true if the Z magnetometer observation has been rejected
 bool reject_yaw                           #  7 - true if the yaw observation has been rejected
 bool reject_airspeed                      #  8 - true if the airspeed observation has been rejected
 bool reject_sideslip                      #  9 - true if the synthetic sideslip observation has been rejected

src/modules/ekf2/EKF/common.h

@@ -438,9 +438,9 @@ union innovation_fault_status_u {
 		bool reject_ver_vel   : 1; ///< 1 - true if vertical velocity observations have been rejected
 		bool reject_hor_pos   : 1; ///< 2 - true if horizontal position observations have been rejected
 		bool reject_ver_pos   : 1; ///< 3 - true if true if vertical position observations have been rejected
-		bool reject_mag_x     : 1; ///< 4 - true if the X magnetometer observation has been rejected
-		bool reject_mag_y     : 1; ///< 5 - true if the Y magnetometer observation has been rejected
-		bool reject_mag_z     : 1; ///< 6 - true if the Z magnetometer observation has been rejected
+		bool __reject_mag_x     : 1; ///< 4 - true if the X magnetometer observation has been rejected
+		bool __reject_mag_y     : 1; ///< 5 - true if the Y magnetometer observation has been rejected
+		bool __reject_mag_z     : 1; ///< 6 - true if the Z magnetometer observation has been rejected
 		bool reject_yaw       : 1; ///< 7 - true if the yaw observation has been rejected
 		bool reject_airspeed  : 1; ///< 8 - true if the airspeed observation has been rejected
 		bool reject_sideslip  : 1; ///< 9 - true if the synthetic sideslip observation has been rejected

src/modules/ekf2/EKF/ekf.h

@@ -112,9 +112,9 @@ public:
 	void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
 	void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
 
-	void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
-	void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
-	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
+	void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
+	void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
+	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
 
 	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
 	void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); }
@@ -345,6 +345,9 @@ public:
 
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
+	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
+	const auto &aid_src_mag() const { return _aid_src_mag; }
+
 	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
 
@@ -462,9 +465,6 @@ private:
 	float _heading_innov{0.0f};	///< heading measurement innovation (rad)
 	float _heading_innov_var{0.0f};	///< heading measurement innovation variance (rad**2)
 
-	Vector3f _mag_innov{};		///< earth magnetic field innovations (Gauss)
-	Vector3f _mag_innov_var{};	///< earth magnetic field innovation variance (Gauss**2)
-
 	Vector2f _drag_innov{};		///< multirotor drag measurement innovation (m/sec**2)
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
 
@@ -496,6 +496,9 @@ private:
 
 	estimator_aid_source_2d_s _aid_src_fake_pos{};
 
+	estimator_aid_source_1d_s _aid_src_mag_heading{};
+	estimator_aid_source_3d_s _aid_src_mag{};
+
 	estimator_aid_source_3d_s _aid_src_gnss_vel{};
 	estimator_aid_source_3d_s _aid_src_gnss_pos{};
 
@@ -597,7 +600,7 @@ private:
 	void predictCovariance();
 
 	// ekf sequential fusion of magnetometer measurements
-	void fuseMag(const Vector3f &mag);
+	void fuseMag(estimator_aid_source_3d_s &aid_src_mag, const Vector3f &mag);
 
 	// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
 	void fuseHeading(float measured_hdg = NAN, float obs_var = NAN);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -910,7 +910,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	status = _innov_check_fail_status.value;
 
 	// return the largest magnetometer innovation test ratio
-	mag = sqrtf(math::max(_yaw_test_ratio, _mag_test_ratio.max()));
+	mag = sqrtf(math::max(_yaw_test_ratio, Vector3f(_aid_src_mag.test_ratio).max()));
 
 	// return the largest velocity and position innovation test ratio
 	vel = NAN;
@@ -983,7 +983,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
 	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
 	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
-	const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
+	const bool mag_innov_good = (Vector3f(_aid_src_mag.test_ratio).max() < 1.0f) && (_yaw_test_ratio < 1.0f);
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
 	soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
 	*status = soln_status.value;
@@ -1236,12 +1236,18 @@ void Ekf::stopMag3DFusion()
 	if (_control_status.flags.mag_3D) {
 		saveMagCovData();
 		_control_status.flags.mag_3D = false;
+
+		resetEstimatorAidStatus(_aid_src_mag);
 	}
 }
 
 void Ekf::stopMagHdgFusion()
 {
-	_control_status.flags.mag_hdg = false;
+	if (_control_status.flags.mag_hdg) {
+		_control_status.flags.mag_hdg = false;
+
+		resetEstimatorAidStatus(_aid_src_mag_heading);
+	}
 }
 
 void Ekf::startMagHdgFusion()

src/modules/ekf2/EKF/estimator_interface.h

@@ -330,7 +330,6 @@ protected:
 	// innovation consistency check monitoring ratios
 	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
 	AlphaFilter<float> _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
-	Vector3f _mag_test_ratio{};		// magnetometer XYZ innovation consistency check ratios
 	Vector2f _ev_vel_test_ratio{};		// EV velocity innovation consistency check ratios
 	Vector2f _ev_pos_test_ratio{};		// EV position innovation consistency check ratios
 	Vector2f _aux_vel_test_ratio{};		// Auxiliary horizontal velocity innovation consistency check ratio

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -147,7 +147,7 @@ void Ekf::fuseGpsYaw()
 	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
 
 	// we are no longer using 3-axis fusion so set the reported test levels to zero
-	_mag_test_ratio.setZero();
+	memset(_aid_src_mag.test_ratio, 0, sizeof(_aid_src_mag.test_ratio));
 
 	if (_yaw_test_ratio > 1.0f) {
 		_innov_check_fail_status.flags.reject_yaw = true;

src/modules/ekf2/EKF/mag_control.cpp

@@ -68,6 +68,10 @@ void Ekf::controlMagFusion()
 	}
 
 	if (mag_data_ready) {
+		// reset flags
+		resetEstimatorAidStatusFlags(_aid_src_mag);
+		resetEstimatorAidStatusFlags(_aid_src_mag_heading);
+
 		checkMagFieldStrength(mag_sample.mag);
 	}
 
@@ -145,6 +149,11 @@ void Ekf::controlMagFusion()
 
 		runMagAndMagDeclFusions(mag_sample.mag);
 	}
+
+	if (mag_data_ready) {
+		_aid_src_mag.timestamp_sample = mag_sample.time_us;
+		_aid_src_mag_heading.timestamp_sample = mag_sample.time_us;
+	}
 }
 
 bool Ekf::noOtherYawAidingThanMag() const
@@ -369,13 +378,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
-		fuseMag(mag);
+		fuseMag(_aid_src_mag, mag);
 
 	} else {
 		// The normal sequence is to fuse the magnetometer data first before fusing
 		// declination angle at a higher uncertainty to allow some learning of
 		// declination angle over time.
-		fuseMag(mag);
+		fuseMag(_aid_src_mag, mag);
 
 		if (_control_status.flags.mag_dec) {
 			fuseDeclination(0.5f);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -45,7 +45,7 @@
 
 #include <mathlib/mathlib.h>
 
-void Ekf::fuseMag(const Vector3f &mag)
+void Ekf::fuseMag(estimator_aid_source_3d_s &aid_src_mag, const Vector3f &mag)
 {
 	// assign intermediate variables
 	const float q0 = _state.quat_nominal(0);
@@ -58,10 +58,11 @@ void Ekf::fuseMag(const Vector3f &mag)
 	const float magD = _state.mag_I(2);
 
 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-	const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
+	const float R_MAG = sq(fmaxf(_params.mag_noise, 0.f));
 
 	// 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";
+	static constexpr const char numerical_error_covariance_reset_string[] {"numerical error - covariance reset"};
+
 	const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
 	const float HKX1 = magD*q3 + magE*q2 + magN*q1;
 	const float HKX2 = magE*q1;
@@ -87,21 +88,9 @@ void Ekf::fuseMag(const Vector3f &mag)
 	const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19);
 	const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19);
 
-	_mag_innov_var(0) = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG;
+	aid_src_mag.innovation_variance[0] = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG;
 
-	if (_mag_innov_var(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
-		resetMagRelatedCovariances();
-		ECL_ERR("magX %s", numerical_error_covariance_reset_string);
-		return;
-	}
-
-	_fault_status.flags.bad_mag_x = false;
-
-	const float HKX24 = 1.0F/_mag_innov_var(0);
+	const float HKX24 = 1.f / aid_src_mag.innovation_variance[0];
 
 	// intermediate variables for calculation of innovations variances for Y and Z axes
 	// don't calculate all terms needed for observation jacobians and Kalman gains because
@@ -126,69 +115,91 @@ void Ekf::fuseMag(const Vector3f &mag)
 	const float IV17 = 2*IV0 - 2*IV1;
 	const float IV18 = IV10 - IV8 + IV9;
 
-	_mag_innov_var(1) = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
-	_mag_innov_var(2) = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
+	aid_src_mag.innovation_variance[1] = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
+	aid_src_mag.innovation_variance[2] = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
 
-	// chedk innovation variances for being badly conditioned
+	// check innovation variances for being badly conditioned
+	if (_control_status.flags.mag_3D) {
 
-	if (_mag_innov_var(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;
+		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
-		resetMagRelatedCovariances();
-		ECL_ERR("magY %s", numerical_error_covariance_reset_string);
-		return;
+			// we need to re-initialise covariances and abort this fusion step
+			resetMagRelatedCovariances();
+			ECL_ERR("magX %s", numerical_error_covariance_reset_string);
+			return;
+
+		} else {
+			_fault_status.flags.bad_mag_x = false;
+		}
+
+		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
+			resetMagRelatedCovariances();
+			ECL_ERR("magY %s", numerical_error_covariance_reset_string);
+			return;
+
+		} else {
+			_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
+			resetMagRelatedCovariances();
+			ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
+			return;
+
+		} else {
+			_fault_status.flags.bad_mag_z = false;
+		}
 	}
 
-	_fault_status.flags.bad_mag_y = false;
-
-	if (_mag_innov_var(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
-		resetMagRelatedCovariances();
-		ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
-		return;
-	}
-
-	_fault_status.flags.bad_mag_z = false;
+	// compute magnetometer innovations
+	// Perform an innovation consistency check and report the result
+	float innov_gate = math::max(_params.mag_innov_gate, 1.f);
 
 	// rotate magnetometer earth field state into body frame
 	const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-
 	const Vector3f mag_I_rot = R_to_body * _state.mag_I;
 
 	// compute magnetometer innovations
-	_mag_innov = mag_I_rot + _state.mag_B - mag;
+	const Vector3f mag_observation = mag - _state.mag_B;
+	const Vector3f mag_innov = mag_I_rot - mag_observation;
+
+	mag_observation.copyTo(aid_src_mag.observation);
+	mag_innov.copyTo(aid_src_mag.innovation);
+
+	for (int i = 0; i < 3; i++) {
+		aid_src_mag.observation_variance[i] = R_MAG;
+		// aid_src_mag.innovation_variance[i] // computed separately
+		aid_src_mag.test_ratio[i] = sq(aid_src_mag.innovation[i]) / (sq(innov_gate) * aid_src_mag.innovation_variance[i]);
+
+		aid_src_mag.innovation_rejected[i] = (aid_src_mag.test_ratio[i] > 1.f);
+	}
 
 	// 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;
-	}
-
-	// Perform an innovation consistency check and report the result
-	bool all_innovation_checks_passed = true;
-
-	for (uint8_t index = 0; index <= 2; index++) {
-		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
-
-		if (_mag_test_ratio(index) > 1.0f) {
-			all_innovation_checks_passed = false;
-			_innov_check_fail_status.value |= (1 << (index + 3));
-
-		} else {
-			_innov_check_fail_status.value &= ~(1 << (index + 3));
-		}
+		aid_src_mag.innovation[2] = 0.0f;
+		aid_src_mag.innovation_rejected[2] = false;
 	}
 
 	// we are no longer using heading fusion so set the reported test level to zero
 	_yaw_test_ratio = 0.0f;
 
 	// if any axis fails, abort the mag fusion
-	if (!all_innovation_checks_passed) {
-		return;
+	if (_control_status.flags.mag_3D) {
+		for (auto& innovation_rejected : aid_src_mag.innovation_rejected) {
+			if (innovation_rejected) {
+				return;
+			}
+		}
 	}
 
 	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
@@ -200,223 +211,251 @@ void Ekf::fuseMag(const Vector3f &mag)
 	Vector24f Kfusion;
 
 	// update the states and covariance using sequential fusion of the magnetometer components
-	for (uint8_t index = 0; index <= 2; index++) {
+	// Calculate Kalman gains and observation jacobians
 
-		// Calculate Kalman gains and observation jacobians
-		if (index == 0) {
-			// Calculate X axis observation jacobians
-			Hfusion.at<0>() = 2*HKX0;
-			Hfusion.at<1>() = 2*HKX1;
-			Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4;
-			Hfusion.at<3>() = 2*HKX5;
-			Hfusion.at<16>() = HKX6;
-			Hfusion.at<17>() = 2*HKX7;
-			Hfusion.at<18>() = 2*HKX8 - 2*HKX9;
-			Hfusion.at<19>() = 1;
+	// Calculate X axis observation jacobians
+	Hfusion.at<0>() = 2*HKX0;
+	Hfusion.at<1>() = 2*HKX1;
+	Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4;
+	Hfusion.at<3>() = 2*HKX5;
+	Hfusion.at<16>() = HKX6;
+	Hfusion.at<17>() = 2*HKX7;
+	Hfusion.at<18>() = 2*HKX8 - 2*HKX9;
+	Hfusion.at<19>() = 1;
 
-			// Calculate X axis Kalman gains
-			if (update_all_states) {
-				Kfusion(0) = HKX16*HKX24;
-				Kfusion(1) = HKX22*HKX24;
-				Kfusion(2) = HKX19*HKX24;
-				Kfusion(3) = HKX21*HKX24;
+	// Calculate X axis Kalman gains
+	if (update_all_states) {
+		Kfusion(0) = HKX16*HKX24;
+		Kfusion(1) = HKX22*HKX24;
+		Kfusion(2) = HKX19*HKX24;
+		Kfusion(3) = HKX21*HKX24;
 
-				for (unsigned row = 4; row <= 15; row++) {
-					Kfusion(row) = HKX24*(HKX10*P(row,17) - HKX11*P(row,18) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(row,16) + P(row,19));
-				}
-
-				for (unsigned row = 22; row <= 23; row++) {
-					Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
-				}
-			}
-
-			Kfusion(16) = HKX17*HKX24;
-			Kfusion(17) = HKX20*HKX24;
-			Kfusion(18) = HKX18*HKX24;
-			Kfusion(19) = HKX23*HKX24;
-
-			for (unsigned row = 20; row <= 21; row++) {
-				Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
-			}
-
-		} else if (index == 1) {
-
-			// recalculate innovation variance becasue states and covariances have changed due to previous fusion
-			const float HKY0 = magD*q1 + magE*q0 - magN*q3;
-			const float HKY1 = magD*q0 - magE*q1 + magN*q2;
-			const float HKY2 = magD*q3 + magE*q2 + magN*q1;
-			const float HKY3 = magD*q2;
-			const float HKY4 = magE*q3;
-			const float HKY5 = magN*q0;
-			const float HKY6 = q1*q2;
-			const float HKY7 = q0*q3;
-			const float HKY8 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
-			const float HKY9 = q0*q1 + q2*q3;
-			const float HKY10 = 2*HKY9;
-			const float HKY11 = -2*HKY6 + 2*HKY7;
-			const float HKY12 = 2*HKY2;
-			const float HKY13 = 2*HKY0;
-			const float HKY14 = 2*HKY1;
-			const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5;
-			const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20);
-			const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20);
-			const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20);
-			const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20);
-			const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20);
-			const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20);
-			const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
-			const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);
-
-			_mag_innov_var(1) = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
-
-			if (_mag_innov_var(1) < 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_y = true;
-
-				// we need to re-initialise covariances and abort this fusion step
-				resetMagRelatedCovariances();
-				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
-				return;
-			}
-			const float HKY24 = 1.0F/_mag_innov_var(1);
-
-			// Calculate Y axis observation jacobians
-			Hfusion.setZero();
-			Hfusion.at<0>() = 2*HKY0;
-			Hfusion.at<1>() = 2*HKY1;
-			Hfusion.at<2>() = 2*HKY2;
-			Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5;
-			Hfusion.at<16>() = 2*HKY6 - 2*HKY7;
-			Hfusion.at<17>() = HKY8;
-			Hfusion.at<18>() = 2*HKY9;
-			Hfusion.at<20>() = 1;
-
-			// Calculate Y axis Kalman gains
-			if (update_all_states) {
-				Kfusion(0) = HKY16*HKY24;
-				Kfusion(1) = HKY21*HKY24;
-				Kfusion(2) = HKY22*HKY24;
-				Kfusion(3) = HKY19*HKY24;
-
-				for (unsigned row = 4; row <= 15; row++) {
-					Kfusion(row) = HKY24*(HKY10*P(row,18) - HKY11*P(row,16) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(row,17) + P(row,20));
-				}
-
-				for (unsigned row = 22; row <= 23; row++) {
-					Kfusion(row) = HKY24*(HKY10*P(18,row) - HKY11*P(16,row) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(17,row) + P(20,row));
-				}
-			}
-
-			Kfusion(16) = HKY18*HKY24;
-			Kfusion(17) = HKY17*HKY24;
-			Kfusion(18) = HKY20*HKY24;
-			Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20));
-			Kfusion(20) = HKY23*HKY24;
-			Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21));
-
-		} else if (index == 2) {
-
-			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
-			if (_control_status.flags.synthetic_mag_z) {
-				continue;
-			}
-
-			// recalculate innovation variance becasue states and covariances have changed due to previous fusion
-			const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
-			const float HKZ1 = magN*q3;
-			const float HKZ2 = magD*q1;
-			const float HKZ3 = magE*q0;
-			const float HKZ4 = -magD*q2 + magE*q3 + magN*q0;
-			const float HKZ5 = magD*q3 + magE*q2 + magN*q1;
-			const float HKZ6 = q0*q2 + q1*q3;
-			const float HKZ7 = q2*q3;
-			const float HKZ8 = q0*q1;
-			const float HKZ9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) - ecl::powf(q2, 2) + ecl::powf(q3, 2);
-			const float HKZ10 = 2*HKZ6;
-			const float HKZ11 = -2*HKZ7 + 2*HKZ8;
-			const float HKZ12 = 2*HKZ5;
-			const float HKZ13 = 2*HKZ0;
-			const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3;
-			const float HKZ15 = 2*HKZ4;
-			const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21);
-			const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21);
-			const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21);
-			const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21);
-			const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21);
-			const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21);
-			const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
-			const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);
-
-			_mag_innov_var(2) = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
-
-			if (_mag_innov_var(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
-				resetMagRelatedCovariances();
-				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
-				return;
-			}
-
-			const float HKZ24 = 1.0F/_mag_innov_var(2);
-
-			// calculate Z axis observation jacobians
-			Hfusion.setZero();
-			Hfusion.at<0>() = 2*HKZ0;
-			Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3;
-			Hfusion.at<2>() = 2*HKZ4;
-			Hfusion.at<3>() = 2*HKZ5;
-			Hfusion.at<16>() = 2*HKZ6;
-			Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8;
-			Hfusion.at<18>() = HKZ9;
-			Hfusion.at<21>() = 1;
-
-			// Calculate Z axis Kalman gains
-			if (update_all_states) {
-				Kfusion(0) = HKZ16*HKZ24;
-				Kfusion(1) = HKZ19*HKZ24;
-				Kfusion(2) = HKZ22*HKZ24;
-				Kfusion(3) = HKZ21*HKZ24;
-
-				for (unsigned row = 4; row <= 15; row++) {
-					Kfusion(row) = HKZ24*(HKZ10*P(row,16) - HKZ11*P(row,17) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(row,18) + P(row,21));
-				}
-
-				for (unsigned row = 22; row <= 23; row++) {
-					Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(21,row));
-				}
-			}
-
-			Kfusion(16) = HKZ20*HKZ24;
-			Kfusion(17) = HKZ18*HKZ24;
-			Kfusion(18) = HKZ17*HKZ24;
-
-			for (unsigned row = 19; row <= 20; row++) {
-				Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(row,21));
-			}
-
-			Kfusion(21) = HKZ23*HKZ24;
+		for (unsigned row = 4; row <= 15; row++) {
+			Kfusion(row) = HKX24*(HKX10*P(row,17) - HKX11*P(row,18) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(row,16) + P(row,19));
 		}
 
-		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index));
-
-		switch (index) {
-		case 0:
-			_fault_status.flags.bad_mag_x = !is_fused;
-			break;
-
-		case 1:
-			_fault_status.flags.bad_mag_y = !is_fused;
-			break;
-
-		case 2:
-			_fault_status.flags.bad_mag_z = !is_fused;
-			break;
+		for (unsigned row = 22; row <= 23; row++) {
+			Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
 		}
+	}
 
-		if (is_fused) {
+	Kfusion(16) = HKX17*HKX24;
+	Kfusion(17) = HKX20*HKX24;
+	Kfusion(18) = HKX18*HKX24;
+	Kfusion(19) = HKX23*HKX24;
+
+	for (unsigned row = 20; row <= 21; row++) {
+		Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
+	}
+
+
+	aid_src_mag.fusion_enabled[0] = _control_status.flags.mag_3D;
+
+	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
+		resetMagRelatedCovariances();
+		ECL_ERR("magX %s", numerical_error_covariance_reset_string);
+		return;
+
+	} else {
+		if (measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[0])) {
+			aid_src_mag.fused[0] = true;
+			aid_src_mag.time_last_fuse[0] = _time_last_imu;
+			_fault_status.flags.bad_mag_x = false;
 			limitDeclination();
+		} else {
+			aid_src_mag.fused[0] = false;
+			_fault_status.flags.bad_mag_x = true;
+		}
+	}
+
+
+	// Y
+	// recalculate innovation variance because states and covariances have changed due to previous fusion
+	const float HKY0 = magD*q1 + magE*q0 - magN*q3;
+	const float HKY1 = magD*q0 - magE*q1 + magN*q2;
+	const float HKY2 = magD*q3 + magE*q2 + magN*q1;
+	const float HKY3 = magD*q2;
+	const float HKY4 = magE*q3;
+	const float HKY5 = magN*q0;
+	const float HKY6 = q1*q2;
+	const float HKY7 = q0*q3;
+	const float HKY8 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
+	const float HKY9 = q0*q1 + q2*q3;
+	const float HKY10 = 2*HKY9;
+	const float HKY11 = -2*HKY6 + 2*HKY7;
+	const float HKY12 = 2*HKY2;
+	const float HKY13 = 2*HKY0;
+	const float HKY14 = 2*HKY1;
+	const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5;
+	const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20);
+	const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20);
+	const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20);
+	const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20);
+	const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20);
+	const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20);
+	const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
+	const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);
+
+	aid_src_mag.innovation_variance[1] = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
+
+	const float HKY24 = 1.f / aid_src_mag.innovation_variance[1];
+
+	// Calculate Y axis observation jacobians
+	Hfusion.setZero();
+	Hfusion.at<0>() = 2*HKY0;
+	Hfusion.at<1>() = 2*HKY1;
+	Hfusion.at<2>() = 2*HKY2;
+	Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5;
+	Hfusion.at<16>() = 2*HKY6 - 2*HKY7;
+	Hfusion.at<17>() = HKY8;
+	Hfusion.at<18>() = 2*HKY9;
+	Hfusion.at<20>() = 1;
+
+	// Calculate Y axis Kalman gains
+	if (update_all_states) {
+		Kfusion(0) = HKY16*HKY24;
+		Kfusion(1) = HKY21*HKY24;
+		Kfusion(2) = HKY22*HKY24;
+		Kfusion(3) = HKY19*HKY24;
+
+		for (unsigned row = 4; row <= 15; row++) {
+			Kfusion(row) = HKY24*(HKY10*P(row,18) - HKY11*P(row,16) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(row,17) + P(row,20));
+		}
+
+		for (unsigned row = 22; row <= 23; row++) {
+			Kfusion(row) = HKY24*(HKY10*P(18,row) - HKY11*P(16,row) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(17,row) + P(20,row));
+		}
+	}
+
+	Kfusion(16) = HKY18*HKY24;
+	Kfusion(17) = HKY17*HKY24;
+	Kfusion(18) = HKY20*HKY24;
+	Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20));
+	Kfusion(20) = HKY23*HKY24;
+	Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21));
+
+	// fuse y
+	aid_src_mag.fusion_enabled[1] = _control_status.flags.mag_3D;
+
+
+	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
+		resetMagRelatedCovariances();
+		ECL_ERR("magY %s", numerical_error_covariance_reset_string);
+		return;
+
+	} else {
+		if (measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[1])) {
+			aid_src_mag.fused[1] = true;
+			aid_src_mag.time_last_fuse[1] = _time_last_imu;
+			_fault_status.flags.bad_mag_y = false;
+			limitDeclination();
+		} else {
+			aid_src_mag.fused[1] = false;
+			_fault_status.flags.bad_mag_y = true;
+		}
+	}
+
+	// Z
+	if (!_control_status.flags.synthetic_mag_z) {
+		// we do not fuse synthesized magnetomter measurements when doing 3D fusion
+
+		// recalculate innovation variance becasue states and covariances have changed due to previous fusion
+		const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
+		const float HKZ1 = magN*q3;
+		const float HKZ2 = magD*q1;
+		const float HKZ3 = magE*q0;
+		const float HKZ4 = -magD*q2 + magE*q3 + magN*q0;
+		const float HKZ5 = magD*q3 + magE*q2 + magN*q1;
+		const float HKZ6 = q0*q2 + q1*q3;
+		const float HKZ7 = q2*q3;
+		const float HKZ8 = q0*q1;
+		const float HKZ9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) - ecl::powf(q2, 2) + ecl::powf(q3, 2);
+		const float HKZ10 = 2*HKZ6;
+		const float HKZ11 = -2*HKZ7 + 2*HKZ8;
+		const float HKZ12 = 2*HKZ5;
+		const float HKZ13 = 2*HKZ0;
+		const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3;
+		const float HKZ15 = 2*HKZ4;
+		const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21);
+		const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21);
+		const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21);
+		const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21);
+		const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21);
+		const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21);
+		const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
+		const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);
+
+		aid_src_mag.innovation_variance[2] = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
+
+		const float HKZ24 = 1.f / aid_src_mag.innovation_variance[2];
+
+		// calculate Z axis observation jacobians
+		Hfusion.setZero();
+		Hfusion.at<0>() = 2*HKZ0;
+		Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3;
+		Hfusion.at<2>() = 2*HKZ4;
+		Hfusion.at<3>() = 2*HKZ5;
+		Hfusion.at<16>() = 2*HKZ6;
+		Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8;
+		Hfusion.at<18>() = HKZ9;
+		Hfusion.at<21>() = 1;
+
+		// Calculate Z axis Kalman gains
+		if (update_all_states) {
+			Kfusion(0) = HKZ16*HKZ24;
+			Kfusion(1) = HKZ19*HKZ24;
+			Kfusion(2) = HKZ22*HKZ24;
+			Kfusion(3) = HKZ21*HKZ24;
+
+			for (unsigned row = 4; row <= 15; row++) {
+				Kfusion(row) = HKZ24*(HKZ10*P(row,16) - HKZ11*P(row,17) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(row,18) + P(row,21));
+			}
+
+			for (unsigned row = 22; row <= 23; row++) {
+				Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(21,row));
+			}
+		}
+
+		Kfusion(16) = HKZ20*HKZ24;
+		Kfusion(17) = HKZ18*HKZ24;
+		Kfusion(18) = HKZ17*HKZ24;
+
+		for (unsigned row = 19; row <= 20; row++) {
+			Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(row,21));
+		}
+
+		Kfusion(21) = HKZ23*HKZ24;
+
+		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
+			resetMagRelatedCovariances();
+			ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
+			return;
+
+		} else {
+			// fuse z
+			aid_src_mag.fusion_enabled[2] = _control_status.flags.mag_3D;
+
+			if (measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[2])) {
+				aid_src_mag.fused[2] = true;
+				aid_src_mag.time_last_fuse[2] = _time_last_imu;
+				_fault_status.flags.bad_mag_z = false;
+				limitDeclination();
+			} else {
+				aid_src_mag.fused[2] = false;
+				_fault_status.flags.bad_mag_z = true;
+			}
 		}
 	}
 }
@@ -651,7 +690,7 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 	_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
 
 	// we are no longer using 3-axis fusion so set the reported test levels to zero
-	_mag_test_ratio.setZero();
+	memset(_aid_src_mag.test_ratio, 0, sizeof(_aid_src_mag.test_ratio)); // TODO
 
 	// set the magnetometer unhealthy if the test fails
 	if (_yaw_test_ratio > 1.0f) {

src/modules/ekf2/EKF2.cpp

@@ -645,6 +645,12 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	// fake position
 	PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
 
+	// mag heading
+	PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);
+
+	// mag
+	PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
+
 	// GNSS velocity & position
 	PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
@@ -1355,9 +1361,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_ver_vel                  = _ekf.innov_check_fail_status_flags().reject_ver_vel;
 		status_flags.reject_hor_pos                  = _ekf.innov_check_fail_status_flags().reject_hor_pos;
 		status_flags.reject_ver_pos                  = _ekf.innov_check_fail_status_flags().reject_ver_pos;
-		status_flags.reject_mag_x                    = _ekf.innov_check_fail_status_flags().reject_mag_x;
-		status_flags.reject_mag_y                    = _ekf.innov_check_fail_status_flags().reject_mag_y;
-		status_flags.reject_mag_z                    = _ekf.innov_check_fail_status_flags().reject_mag_z;
 		status_flags.reject_yaw                      = _ekf.innov_check_fail_status_flags().reject_yaw;
 		status_flags.reject_airspeed                 = _ekf.innov_check_fail_status_flags().reject_airspeed;
 		status_flags.reject_sideslip                 = _ekf.innov_check_fail_status_flags().reject_sideslip;

src/modules/ekf2/EKF2.hpp

@@ -260,6 +260,9 @@ private:
 
 	hrt_abstime _status_fake_pos_pub_last{0};
 
+	hrt_abstime _status_mag_heading_pub_last{0};
+	hrt_abstime _status_mag_pub_last{0};
+
 	hrt_abstime _status_gnss_vel_pub_last{0};
 	hrt_abstime _status_gnss_pos_pub_last{0};
 
@@ -327,6 +330,9 @@ private:
 
 	uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
 
+	uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
+	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
+
 	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
 	uORB::PublicationMulti<estimator_aid_source_3d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};