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/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)};