ekf2: use estimator_aid_src for all yaw sources (mag, gnss, ev)

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 estimator_aid_src_airspeed
+# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw

msg/estimator_aid_source_3d.msg

@@ -19,4 +19,4 @@ bool[3] innovation_rejected  # true if the observation has been rejected
 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_gnss_pos estimator_aid_src_gnss_vel 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/control.cpp

@@ -361,21 +361,41 @@ void Ekf::controlExternalVisionFusion()
 		}
 
 		// determine if we should use the yaw observation
-		if (_control_status.flags.ev_yaw) {
+		resetEstimatorAidStatus(_aid_src_ev_yaw);
-			if (reset && _control_status_prev.flags.ev_yaw) {
+		const float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
-				resetYawToEv();
+		const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
+
+		if (PX4_ISFINITE(measured_hdg)) {
+			_aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us;
+			_aid_src_ev_yaw.observation = measured_hdg;
+			_aid_src_ev_yaw.observation_variance = ev_yaw_obs_var;
+			_aid_src_ev_yaw.fusion_enabled = _control_status.flags.ev_yaw;
+
+			if (_control_status.flags.ev_yaw) {
+				if (reset && _control_status_prev.flags.ev_yaw) {
+					resetYawToEv();
+				}
+
+				const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
+
+				fuseYaw(innovation, ev_yaw_obs_var, _aid_src_ev_yaw);
+
+			} else {
+				// populate estimator_aid_src_ev_yaw with delta heading innovations for logging
+				// use the change in yaw since the last measurement
+				const float measured_hdg_prev = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed_prev.quat) : getEuler312Yaw(_ev_sample_delayed_prev.quat);
+
+				// calculate the change in yaw since the last measurement
+				const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev);
+
+				_aid_src_ev_yaw.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yaw_pred_prev - ev_delta_yaw);
 			}
-
-			float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
-
-			float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
-			float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
-			fuseYaw(innovation, obs_var);
 		}
 
 		// record observation and estimate for use next time
 		_ev_sample_delayed_prev = _ev_sample_delayed;
 		_hpos_pred_prev = _state.pos.xy();
+		_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
 
 	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel ||  _control_status.flags.ev_yaw)
 		   && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
@@ -584,7 +604,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 
 				fuseGpsYaw();
 
-				const bool is_fusion_failing = isTimedOut(_time_last_gps_yaw_fuse, _params.reset_timeout_max);
+				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
 					if (_nb_gps_yaw_reset_available > 0) {

src/modules/ekf2/EKF/ekf.h

@@ -111,13 +111,54 @@ public:
 	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
 	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
 
-	void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
+	void getHeadingInnov(float &heading_innov) const {
-	void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
+		if (_control_status.flags.mag_hdg) {
-	void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
+			heading_innov = _aid_src_mag_heading.innovation;
 
-	void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
+		} else if (_control_status.flags.mag_3D) {
-	void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
+			heading_innov = Vector3f(_aid_src_mag.innovation).max();
-	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
+
+		} else if (_control_status.flags.gps_yaw) {
+			heading_innov = _aid_src_gnss_yaw.innovation;
+
+		} else if (_control_status.flags.ev_yaw) {
+			heading_innov = _aid_src_ev_yaw.innovation;
+		}
+	}
+
+	void getHeadingInnovVar(float &heading_innov_var) const {
+		if (_control_status.flags.mag_hdg) {
+			heading_innov_var = _aid_src_mag_heading.innovation_variance;
+
+		} else if (_control_status.flags.mag_3D) {
+			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
+
+		} else if (_control_status.flags.gps_yaw) {
+			heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
+
+		} else if (_control_status.flags.ev_yaw) {
+			heading_innov_var = _aid_src_ev_yaw.innovation_variance;
+		}
+	}
+
+	void getHeadingInnovRatio(float &heading_innov_ratio) const {
+		if (_control_status.flags.mag_hdg) {
+			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
+
+		} else if (_control_status.flags.mag_3D) {
+			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
+
+		} else if (_control_status.flags.gps_yaw) {
+			heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
+
+		} else if (_control_status.flags.ev_yaw) {
+			heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
+		}
+	}
+
+	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); }
@@ -354,9 +395,15 @@ public:
 
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
+	const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
+
+	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
 	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
 
+	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
+	const auto &aid_src_mag() const { return _aid_src_mag; }
+
 private:
 
 	// set the internal states and status to their default value
@@ -391,6 +438,7 @@ private:
 	// variables used when position data is being fused using a relative position odometry model
 	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
 	Vector2f _hpos_pred_prev{};		///< previous value of NE position state used by odometry fusion (m)
+	float _yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
 	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
 	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
 	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
@@ -418,10 +466,7 @@ private:
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
-	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
-	uint64_t _time_last_mag_heading_fuse{0};
-	uint64_t _time_last_mag_3d_fuse{0};
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
@@ -470,12 +515,6 @@ private:
 	Vector3f _aux_vel_innov{};	///< horizontal auxiliary velocity innovations: (m/sec)
 	Vector3f _aux_vel_innov_var{};	///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
 
-	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)
 
@@ -505,9 +544,15 @@ private:
 
 	estimator_aid_source_2d_s _aid_src_fake_pos{};
 
+	estimator_aid_source_1d_s _aid_src_ev_yaw{};
+
+	estimator_aid_source_1d_s _aid_src_gnss_yaw{};
 	estimator_aid_source_3d_s _aid_src_gnss_vel{};
 	estimator_aid_source_3d_s _aid_src_gnss_pos{};
 
+	estimator_aid_source_1d_s _aid_src_mag_heading{};
+	estimator_aid_source_3d_s _aid_src_mag{};
+
 	// output predictor states
 	Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
 	Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
@@ -606,12 +651,12 @@ private:
 	void predictCovariance();
 
 	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, bool update_all_states = true);
+	bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true);
 
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	// innovation : prediction - measurement
 	// variance : observaton variance
-	bool fuseYaw(const float innovation, const float variance);
+	bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status);
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();
@@ -686,10 +731,11 @@ private:
 	bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
 				  float &innov_var, float &test_ratio);
 
+	void updateGpsYaw(const gpsSample &gps_sample);
 	void updateGpsVel(const gpsSample &gps_sample);
-	void fuseGpsVel();
-
 	void updateGpsPos(const gpsSample &gps_sample);
+
+	void fuseGpsVel();
 	void fuseGpsPos();
 
 	// calculate optical flow body angular rate compensation

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -931,7 +931,17 @@ 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()));
+	if (_control_status.flags.mag_hdg) {
+		mag = sqrtf(_aid_src_mag_heading.test_ratio);
+
+	} else if (_control_status.flags.mag_3D) {
+		mag = sqrtf(Vector3f(_aid_src_mag.test_ratio).max());
+
+	} else if (_control_status.flags.gps_yaw) {
+		mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
+	} else {
+		mag = NAN;
+	}
 
 	// return the largest velocity and position innovation test ratio
 	vel = NAN;
@@ -1002,9 +1012,23 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
+
+	bool mag_innov_good = true;
+
+	if (_control_status.flags.mag_hdg) {
+		if (_aid_src_mag_heading.test_ratio < 1.f) {
+			mag_innov_good = false;
+		}
+
+	} else if (_control_status.flags.mag_3D) {
+		if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) {
+			mag_innov_good = false;
+		}
+	}
+
 	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);
+
 	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;
@@ -1260,10 +1284,6 @@ void Ekf::stopMag3DFusion()
 		_control_status.flags.mag_3D = false;
 		_control_status.flags.mag_dec = false;
 
-		_mag_innov.zero();
-		_mag_innov_var.zero();
-		_mag_test_ratio.zero();
-
 		_fault_status.flags.bad_mag_x = false;
 		_fault_status.flags.bad_mag_y = false;
 		_fault_status.flags.bad_mag_z = false;
@@ -1278,8 +1298,6 @@ void Ekf::stopMagHdgFusion()
 		_control_status.flags.mag_hdg = false;
 
 		_fault_status.flags.bad_hdg = false;
-
-		_yaw_test_ratio = 0.f;
 	}
 }
 
@@ -1298,8 +1316,6 @@ void Ekf::startMag3DFusion()
 
 		stopMagHdgFusion();
 
-		_yaw_test_ratio = 0.0f;
-
 		zeroMagCov();
 		loadMagCovData();
 		_control_status.flags.mag_3D = true;
@@ -1679,6 +1695,7 @@ void Ekf::stopGpsYawFusion()
 	if (_control_status.flags.gps_yaw) {
 		ECL_INFO("stopping GPS yaw fusion");
 		_control_status.flags.gps_yaw = false;
+		resetEstimatorAidStatus(_aid_src_gnss_yaw);
 	}
 }
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -328,9 +328,7 @@ protected:
 	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
 
 	// innovation consistency check monitoring ratios
-	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
+	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
-	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_control.cpp

@@ -49,10 +49,7 @@ void Ekf::controlGpsFusion()
 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {
 
-		// reset flags
+		updateGpsYaw(_gps_sample_delayed);
-		resetEstimatorAidStatusFlags(_aid_src_gnss_vel);
-		resetEstimatorAidStatusFlags(_aid_src_gnss_pos);
-
 		updateGpsVel(_gps_sample_delayed);
 		updateGpsPos(_gps_sample_delayed);
 

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,16 +39,43 @@
 /* #include <mathlib/mathlib.h> */
 #include "ekf.h"
 
+void Ekf::updateGpsYaw(const gpsSample &gps_sample)
+{
+	auto &gps_yaw = _aid_src_gnss_yaw;
+	resetEstimatorAidStatusFlags(gps_yaw);
+
+	if (PX4_ISFINITE(gps_sample.yaw)) {
+		// initially populate for estimator_aid_src_gnss_yaw logging
+
+		// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
+		const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
+
+		gps_yaw.observation = measured_hdg;
+		gps_yaw.observation_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
+
+		// define the predicted antenna array vector and rotate into earth frame
+		const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+		const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+		const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));
+		gps_yaw.innovation = wrap_pi(predicted_hdg - gps_yaw.observation);
+
+		gps_yaw.fusion_enabled = _control_status.flags.gps_yaw;
+
+		gps_yaw.timestamp_sample = gps_sample.time_us;
+	}
+}
+
 void Ekf::updateGpsVel(const gpsSample &gps_sample)
 {
+	auto &gps_vel = _aid_src_gnss_vel;
+	resetEstimatorAidStatus(gps_vel);
+
 	const float vel_var = sq(gps_sample.sacc);
 	const Vector3f obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
 
 	// innovation gate size
 	const float innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
 
-	auto &gps_vel = _aid_src_gnss_vel;
-
 	for (int i = 0; i < 3; i++) {
 		gps_vel.observation[i] = gps_sample.vel(i);
 		gps_vel.observation_variance[i] = obs_var(i);
@@ -72,6 +99,9 @@ void Ekf::updateGpsVel(const gpsSample &gps_sample)
 
 void Ekf::updateGpsPos(const gpsSample &gps_sample)
 {
+	auto &gps_pos = _aid_src_gnss_pos;
+	resetEstimatorAidStatus(gps_pos);
+
 	Vector3f position;
 	position(0) = gps_sample.pos(0);
 	position(1) = gps_sample.pos(1);
@@ -100,8 +130,6 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 	// innovation gate size
 	float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
 
-	auto &gps_pos = _aid_src_gnss_pos;
-
 	for (int i = 0; i < 3; i++) {
 		gps_pos.observation[i] = position(i);
 		gps_pos.observation_variance[i] = obs_var(i);

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -60,18 +60,27 @@ void Ekf::fuseGpsYaw()
 	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
 	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
 
-	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
-		return;
-	}
-
 	// calculate predicted antenna yaw angle
 	const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));
 
+	// wrap the innovation to the interval between +-pi
+	const float heading_innov = wrap_pi(predicted_hdg - measured_hdg);
+
 	// using magnetic heading process noise
 	// TODO extend interface to use yaw uncertainty provided by GPS if available
 	const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
 
+	_aid_src_gnss_yaw.timestamp_sample = _gps_sample_delayed.time_us;
+	_aid_src_gnss_yaw.observation = measured_hdg;
+	_aid_src_gnss_yaw.observation_variance = R_YAW;
+	_aid_src_gnss_yaw.innovation = heading_innov;
+	_aid_src_gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw;
+
+	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+		return;
+	}
+
 	// calculate intermediate variables
 	const float HK0 = sinf(_gps_yaw_offset);
 	const float HK1 = q0*q3;
@@ -121,9 +130,11 @@ void Ekf::fuseGpsYaw()
 	// const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
 
 	// check if the innovation variance calculation is badly conditioned
-	_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
+	const float heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
 
-	if (_heading_innov_var < R_YAW) {
+	_aid_src_gnss_yaw.innovation_variance = heading_innov_var;
+
+	if (heading_innov_var < R_YAW) {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_hdg = true;
 
@@ -134,19 +145,15 @@ void Ekf::fuseGpsYaw()
 	}
 
 	_fault_status.flags.bad_hdg = false;
-	const float HK32 = HK18 / _heading_innov_var;
+	const float HK32 = HK18 / heading_innov_var;
 
 	// calculate the innovation and define the innovation gate
 	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-	_heading_innov = predicted_hdg - measured_hdg;
-
-	// wrap the innovation to the interval between +-pi
-	_heading_innov = wrap_pi(_heading_innov);
 
 	// innovation test ratio
-	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
+	setEstimatorAidStatusTestRatio(_aid_src_gnss_yaw, innov_gate);
 
-	if (_yaw_test_ratio > 1.0f) {
+	if (_aid_src_gnss_yaw.innovation_rejected) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 		return;
 
@@ -154,10 +161,10 @@ void Ekf::fuseGpsYaw()
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
-	_yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio);
+	_gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(heading_innov) * _aid_src_gnss_yaw.test_ratio);
 
-	if ((fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
+	if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
-		&& !_control_status.flags.in_air && isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)) {
+		&& !_control_status.flags.in_air && isTimedOut(_aid_src_gnss_yaw.time_last_fuse, (uint64_t)1e6)) {
 
 		// A constant large signed test ratio is a sign of wrong gyro bias
 		// Reset the yaw gyro variance to converge faster and avoid
@@ -184,12 +191,13 @@ void Ekf::fuseGpsYaw()
 		Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row));
 	}
 
-	const bool is_fused = measurementUpdate(Kfusion, Hfusion, _heading_innov);
+	const bool is_fused = measurementUpdate(Kfusion, Hfusion, heading_innov);
 	_fault_status.flags.bad_hdg = !is_fused;
+	_aid_src_gnss_yaw.fused = is_fused;
 
 	if (is_fused) {
-		_time_last_gps_yaw_fuse = _time_last_imu;
 		_time_last_heading_fuse = _time_last_imu;
+		_aid_src_gnss_yaw.time_last_fuse = _time_last_imu;
 	}
 }
 
@@ -207,11 +215,11 @@ bool Ekf::resetYawToGps()
 	// GPS yaw measurement is alreday compensated for antenna offset in the driver
 	const float measured_yaw = _gps_sample_delayed.yaw;
 
-	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
+	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance, true);
 
-	_time_last_gps_yaw_fuse = _time_last_imu;
+	_aid_src_gnss_yaw.time_last_fuse = _time_last_imu;
-	_yaw_signed_test_ratio_lpf.reset(0.f);
+	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
 
 	return true;
 }

src/modules/ekf2/EKF/mag_control.cpp

@@ -66,6 +66,28 @@ void Ekf::controlMagFusion()
 			}
 
 			_control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag);
+
+
+			// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
+			const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
+			const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
+			const Vector3f mag_earth_pred = R_to_earth * mag_observation;
+
+			resetEstimatorAidStatus(_aid_src_mag_heading);
+			_aid_src_mag_heading.timestamp_sample = mag_sample.time_us;
+			_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();;
+			_aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation);
+
+			// compute magnetometer innovations (for estimator_aid_src_mag logging)
+			//  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;
+			const Vector3f mag_innov = mag_I_rot - mag_observation;
+
+			resetEstimatorAidStatus(_aid_src_mag);
+			_aid_src_mag.timestamp_sample = mag_sample.time_us;
+			mag_observation.copyTo(_aid_src_mag.observation);
+			mag_innov.copyTo(_aid_src_mag.innovation);
 		}
 	}
 
@@ -344,9 +366,9 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 		float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
 		float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
 
-		if (fuseYaw(innovation, obs_var)) {
+		_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
-			_time_last_mag_heading_fuse = _time_last_imu;
+
-		}
+		fuseYaw(innovation, obs_var, _aid_src_mag_heading);
 	}
 }
 
@@ -364,13 +386,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
-		fuseMag(mag, update_all_states);
+		fuseMag(mag, _aid_src_mag, update_all_states);
 
 	} 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, update_all_states);
+		fuseMag(mag, _aid_src_mag, update_all_states);
 
 		if (_control_status.flags.mag_dec) {
 			fuseDeclination(0.5f);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -45,7 +45,7 @@
 
 #include <mathlib/mathlib.h>
 
-bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
+bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states)
 {
 	// assign intermediate variables
 	const float q0 = _state.quat_nominal(0);
@@ -87,9 +87,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 	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) {
+	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;
 
@@ -101,7 +101,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 
 	_fault_status.flags.bad_mag_x = false;
 
-	const float HKX24 = 1.0F/_mag_innov_var(0);
+	const float HKX24 = 1.0F/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,12 +126,11 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 	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;
+	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;
-	_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[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 (aid_src_mag.innovation_variance[1] < R_MAG) {
-	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;
 
@@ -143,7 +142,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 
 	_fault_status.flags.bad_mag_y = false;
 
-	if (_mag_innov_var(2) < R_MAG) {
+	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;
 
@@ -157,46 +156,40 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 
 	// 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;
+	Vector3f mag_innov = mag_I_rot - mag_observation;
 
 	// 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;
+		mag_innov(2) = 0.0f;
 	}
 
+	for (int i = 0; i < 3; i++) {
+		aid_src_mag.observation[i] = mag_observation(i);
+		aid_src_mag.observation_variance[i] = R_MAG;
+		aid_src_mag.innovation[i] = mag_innov(i);
+		aid_src_mag.fusion_enabled[i] = _control_status.flags.mag_3D && update_all_states;
+	}
+
+	// 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_rejected[2] = false;
+	}
+
+	const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
+	setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
+
 	// Perform an innovation consistency check and report the result
-	bool all_innovation_checks_passed = true;
+	_innov_check_fail_status.flags.reject_mag_x = aid_src_mag.innovation_rejected[0];
-
+	_innov_check_fail_status.flags.reject_mag_y = aid_src_mag.innovation_rejected[1];
-	for (uint8_t index = 0; index <= 2; index++) {
+	_innov_check_fail_status.flags.reject_mag_z = aid_src_mag.innovation_rejected[2];
-		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
-
-		bool rejected = (_mag_test_ratio(index) > 1.f);
-
-		switch (index) {
-		case 0:
-			_innov_check_fail_status.flags.reject_mag_x = rejected;
-			break;
-
-		case 1:
-			_innov_check_fail_status.flags.reject_mag_y = rejected;
-			break;
-
-		case 2:
-			_innov_check_fail_status.flags.reject_mag_z = rejected;
-			break;
-		}
-
-		if (rejected) {
-			all_innovation_checks_passed = false;
-		}
-	}
 
 	// if any axis fails, abort the mag fusion
-	if (!all_innovation_checks_passed) {
+	if (aid_src_mag.innovation_rejected[0] || aid_src_mag.innovation_rejected[1] || aid_src_mag.innovation_rejected[2]) {
 		return false;
 	}
 
@@ -246,7 +239,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 
 		} else if (index == 1) {
 
-			// recalculate innovation variance becasue states and covariances have changed due to previous fusion
+			// 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;
@@ -272,9 +265,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 			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);
+			aid_src_mag.innovation_variance[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) {
+			if (aid_src_mag.innovation_variance[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;
 
@@ -283,7 +276,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
 				return false;
 			}
-			const float HKY24 = 1.0F/_mag_innov_var(1);
+			const float HKY24 = 1.0F/aid_src_mag.innovation_variance[1];
 
 			// Calculate Y axis observation jacobians
 			Hfusion.setZero();
@@ -352,9 +345,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 			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);
+			aid_src_mag.innovation_variance[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) {
+			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;
 
@@ -364,7 +357,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 				return false;
 			}
 
-			const float HKZ24 = 1.0F/_mag_innov_var(2);
+			const float HKZ24 = 1.0F/aid_src_mag.innovation_variance[2];
 
 			// calculate Z axis observation jacobians
 			Hfusion.setZero();
@@ -404,7 +397,16 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 			Kfusion(21) = HKZ23*HKZ24;
 		}
 
-		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index));
+		const bool is_fused = measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index]);
+
+		if (is_fused) {
+			aid_src_mag.fused[index] = true;
+			aid_src_mag.time_last_fuse[index] = _time_last_imu;
+
+		} else {
+			aid_src_mag.fused[index] = false;
+		}
+
 
 		switch (index) {
 		case 0:
@@ -425,17 +427,14 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 		}
 	}
 
-	if (!_fault_status.flags.bad_mag_x && !_fault_status.flags.bad_mag_y && !_fault_status.flags.bad_mag_z) {
+	return aid_src_mag.fused[0] && aid_src_mag.fused[1] && aid_src_mag.fused[2];
-		_time_last_mag_3d_fuse = _time_last_imu;
-		return true;
-	}
-
-	return false;
 }
 
 // update quaternion states and covariances using the yaw innovation and yaw observation variance
-bool Ekf::fuseYaw(const float innovation, const float variance)
+bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status)
 {
+	aid_src_status.innovation = innovation;
+
 	// assign intermediate state variables
 	const float q0 = _state.quat_nominal(0);
 	const float q1 = _state.quat_nominal(1);
@@ -551,7 +550,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 
 	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
 	// calculate the innovation variance
-	_heading_innov_var = variance;
+	aid_src_status.innovation_variance = variance;
 
 	for (unsigned row = 0; row <= 3; row++) {
 		float tmp = 0.0f;
@@ -560,16 +559,16 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 			tmp += P(row, col) * H_YAW(col);
 		}
 
-		_heading_innov_var += H_YAW(row) * tmp;
+		aid_src_status.innovation_variance += H_YAW(row) * tmp;
 	}
 
-	float heading_innov_var_inv;
+	float heading_innov_var_inv = 0.f;
 
 	// check if the innovation variance calculation is badly conditioned
-	if (_heading_innov_var >= variance) {
+	if (aid_src_status.innovation_variance >= variance) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
 		_fault_status.flags.bad_hdg = false;
-		heading_innov_var_inv = 1.0f / _heading_innov_var;
+		heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
 
 	} else {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
@@ -607,10 +606,10 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
 
 	// innovation test ratio
-	_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
+	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
 
 	// set the magnetometer unhealthy if the test fails
-	if (_yaw_test_ratio > 1.0f) {
+	if (aid_src_status.innovation_rejected) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 
 		// if we are in air we don't want to fuse the measurement
@@ -618,13 +617,13 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 		// by interference or a large initial gyro bias
 		if (!_control_status.flags.in_air
 		&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
-		&& isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)
+		&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
 		) {
 			// constrain the innovation to the maximum set by the gate
 			// we need to delay this forced fusion to avoid starting it
 			// immediately after touchdown, when the drone is still armed
-			float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var));
+			float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
-			_heading_innov = math::constrain(innovation, -gate_limit, gate_limit);
+			aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
 
 			// also reset the yaw gyro variance to converge faster and avoid
 			// being stuck on a previous bad estimate
@@ -636,7 +635,6 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 
 	} else {
 		_innov_check_fail_status.flags.reject_yaw = false;
-		_heading_innov = innovation;
 	}
 
 	// apply covariance correction via P_new = (I -K*H)*P
@@ -672,9 +670,11 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 		fixCovarianceErrors(true);
 
 		// apply the state corrections
-		fuse(Kfusion, _heading_innov);
+		fuse(Kfusion, aid_src_status.innovation);
 
 		_time_last_heading_fuse = _time_last_imu;
+		aid_src_status.time_last_fuse = _time_last_imu;
+		aid_src_status.fused = true;
 
 		return true;
 	}

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -47,7 +47,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 		// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
 		float innovation = 0.f;
 		float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
-		fuseYaw(innovation, obs_var);
+		estimator_aid_source_1d_s unused;
+		fuseYaw(innovation, obs_var, unused);
 
 		_last_static_yaw = NAN;
 
@@ -61,7 +62,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 			if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
 				float innovation = wrap_pi(euler_yaw - _last_static_yaw);
 				float obs_var = 0.01f;
-				fuseYaw(innovation, obs_var);
+				estimator_aid_source_1d_s unused;
+				fuseYaw(innovation, obs_var, unused);
 			}
 
 		} else {
@@ -76,7 +78,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 		if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
 			float innovation = 0.f;
 			float obs_var = 0.01f;
-			fuseYaw(innovation, obs_var);
+			estimator_aid_source_1d_s unused;
+			fuseYaw(innovation, obs_var, unused);
 		}
 
 		_last_static_yaw = NAN;

src/modules/ekf2/EKF2.cpp

@@ -650,10 +650,19 @@ 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);
 
-	// GNSS velocity & position
+	// EV yaw
+	PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
+
+	// GNSS yaw/velocity/position
+	PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
 	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);
 
+	// mag heading
+	PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);
+
+	// mag 3d
+	PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
 }
 
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -1368,9 +1377,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

@@ -257,9 +257,15 @@ private:
 
 	hrt_abstime _status_fake_pos_pub_last{0};
 
+	hrt_abstime _status_ev_yaw_pub_last{0};
+
+	hrt_abstime _status_gnss_yaw_pub_last{0};
 	hrt_abstime _status_gnss_vel_pub_last{0};
 	hrt_abstime _status_gnss_pos_pub_last{0};
 
+	hrt_abstime _status_mag_pub_last{0};
+	hrt_abstime _status_mag_heading_pub_last{0};
+
 	float _last_baro_bias_published{};
 
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
@@ -324,9 +330,16 @@ 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_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
 	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)};
 
+	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_1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
+
+
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;

src/modules/logger/logged_topics.cpp

@@ -171,6 +171,17 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES);
 
+	// add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
+
 	// log all raw sensors at minimal rate (at least 1 Hz)
 	add_topic_multi("battery_status", 200, 2);
 	add_topic_multi("differential_pressure", 1000, 2);
@@ -245,6 +256,18 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_local_position");
 	add_topic("wind");
 	add_topic("yaw_estimator_status");
+
+	add_optional_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
+
 #endif /* CONFIG_ARCH_BOARD_PX4_SITL */
 }