ekf2: add kconfig for magnetometer support (enabled by default)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -49,12 +49,9 @@ list(APPEND EKF_SRCS
 	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
-	mag_3d_control.cpp
-	mag_control.cpp
-	mag_fusion.cpp
-	mag_heading_control.cpp
 	output_predictor.cpp
 	vel_pos_fusion.cpp
+	yaw_fusion.cpp
 	zero_innovation_heading_update.cpp
 	zero_gyro_update.cpp
 	zero_velocity_update.cpp
@@ -86,6 +83,15 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_MAGNETOMETER)
+	list(APPEND EKF_SRCS
+		mag_3d_control.cpp
+		mag_control.cpp
+		mag_fusion.cpp
+		mag_heading_control.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
 		optical_flow_control.cpp

src/modules/ekf2/EKF/control.cpp

@@ -102,8 +102,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 		}
 	}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control use of observations for aiding
 	controlMagFusion();
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	controlOpticalFlowFusion(imu_delayed);

src/modules/ekf2/EKF/covariance.cpp

@@ -178,23 +178,21 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 
 	// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_I_sig;
+	float mag_I_sig = 0.0f;
 
 	if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
-
-	} else {
-		mag_I_sig = 0.0f;
+#endif // CONFIG_EKF2_MAGNETOMETER
 	}
 
 	// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_B_sig;
+	float mag_B_sig = 0.0f;
 
 	if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
-
-	} else {
-		mag_B_sig = 0.0f;
+#endif // CONFIG_EKF2_MAGNETOMETER
 	}
 
 	float wind_vel_nsd_scaled;
@@ -552,6 +550,7 @@ void Ekf::resetQuatCov(const float yaw_noise)
 
 void Ekf::resetMagCov()
 {
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_mag_decl_cov_reset) {
 		ECL_INFO("reset mag covariance");
 		_mag_decl_cov_reset = false;
@@ -561,6 +560,11 @@ void Ekf::resetMagCov()
 	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
 
 	saveMagCovData();
+#else
+	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
+	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
+
+#endif
 }
 
 void Ekf::resetGyroBiasZCov()

src/modules/ekf2/EKF/ekf.cpp

@@ -110,7 +110,10 @@ void Ekf::reset()
 	_gps_alt_ref = NAN;
 
 	_baro_counter = 0;
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_mag_counter = 0;
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	_time_bad_vert_accel = 0;
 	_time_good_vert_accel = 0;
@@ -142,8 +145,10 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_gnss_yaw);
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	resetEstimatorAidStatus(_aid_src_mag_heading);
 	resetEstimatorAidStatus(_aid_src_mag);
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	resetEstimatorAidStatus(_aid_src_aux_vel);

src/modules/ekf2/EKF/ekf.h

@@ -158,6 +158,7 @@ public:
 
 	void getHeadingInnov(float &heading_innov) const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov = _aid_src_mag_heading.innovation;
 			return;
@@ -166,6 +167,7 @@ public:
 			heading_innov = Vector3f(_aid_src_mag.innovation).max();
 			return;
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -184,6 +186,7 @@ public:
 
 	void getHeadingInnovVar(float &heading_innov_var) const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_var = _aid_src_mag_heading.innovation_variance;
 			return;
@@ -192,6 +195,7 @@ public:
 			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
 			return;
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -210,6 +214,7 @@ public:
 
 	void getHeadingInnovRatio(float &heading_innov_ratio) const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
 			return;
@@ -218,6 +223,7 @@ public:
 			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
 			return;
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -234,9 +240,11 @@ public:
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	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(); }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
@@ -351,12 +359,16 @@ public:
 
 	bool isYawFinalAlignComplete() const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
 		const bool is_mag_alignment_in_flight_complete = is_using_mag
 				&& _control_status.flags.mag_aligned_in_flight
 				&& ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6);
 		return _control_status.flags.yaw_align
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
+#else
+		return _control_status.flags.yaw_align;
+#endif
 	}
 
 	// gyro bias (states 10, 11, 12)
@@ -369,6 +381,7 @@ public:
 	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2
 	float getAccelBiasLimit() const { return _params.acc_bias_lim; }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	const Vector3f &getMagEarthField() const { return _state.mag_I; }
 
 	// mag bias (states 19, 20, 21)
@@ -382,6 +395,7 @@ public:
 		return _saved_mag_bf_covmat.diag();
 	}
 	float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
 	bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; }
@@ -497,8 +511,10 @@ public:
 	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
 	const auto &aid_src_mag() const { return _aid_src_mag; }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	const auto &aid_src_gravity() const { return _aid_src_gravity; }
 
@@ -545,9 +561,6 @@ private:
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
 
-	float _mag_heading_prev{};                 ///< previous value of mag heading (rad)
-	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)
-
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 
@@ -574,19 +587,9 @@ private:
 	Vector3f _zgup_delta_ang{};
 	float _zgup_delta_ang_dt{0.f};
 
-	// used by magnetometer fusion mode selection
 	Vector2f _accel_lpf_NE{};			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
 	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)
-	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
-	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
-	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
-	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
-	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
-	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
-	uint8_t _nb_mag_heading_reset_available{0};
-	uint8_t _nb_mag_3d_reset_available{0};
-	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
 
 	SquareMatrix24f P{};	///< state covariance matrix
 
@@ -668,9 +671,6 @@ private:
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 #endif // CONFIG_EKF2_GNSS_YAW
 
-	estimator_aid_source1d_s _aid_src_mag_heading{};
-	estimator_aid_source3d_s _aid_src_mag{};
-
 	estimator_aid_source3d_s _aid_src_gravity{};
 
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -694,20 +694,40 @@ private:
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
 	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
-	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
 	// Variables used to perform in flight resets and switch between height sources
-	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
 	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	float _mag_heading_prev{};                 ///< previous value of mag heading (rad)
+	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)
+
+	// used by magnetometer fusion mode selection
+	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
+	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
+	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
+	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
+	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
+	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
+	uint8_t _nb_mag_heading_reset_available{0};
+	uint8_t _nb_mag_3d_reset_available{0};
+	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
+
+	estimator_aid_source1d_s _aid_src_mag_heading{};
+	estimator_aid_source3d_s _aid_src_mag{};
+
+	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
+	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
+
 	// Variables used to control activation of post takeoff functionality
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	uint64_t _time_last_mag_check_failing{0};
 	Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
 	Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
@@ -744,9 +764,6 @@ private:
 	// predict ekf covariance
 	void predictCovariance(const imuSample &imu_delayed);
 
-	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
-
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
@@ -767,12 +784,17 @@ private:
 #endif // CONFIG_EKF2_GNSS_YAW
 	void stopGpsYawFusion();
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	// ekf sequential fusion of magnetometer measurements
+	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
+
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
 	bool fuseDeclination(float decl_sigma);
 
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	// control fusion of air data observations
@@ -899,8 +921,10 @@ private:
 	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	void clearInhibitedStateKalmanGains(Vector24f &K) const
 	{
@@ -1017,6 +1041,7 @@ private:
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 	void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src);
@@ -1035,6 +1060,19 @@ private:
 	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
 
+	void stopMagHdgFusion();
+	void stopMagFusion();
+
+	// load and save mag field state covariance data for re-use
+	void loadMagCovData();
+	void saveMagCovData();
+
+	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
+	// sensor measurement
+	float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
+
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 
@@ -1064,9 +1102,6 @@ private:
 	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
 
-	void stopMagHdgFusion();
-	void stopMagFusion();
-
 	void stopBaroHgtFusion();
 	void stopGpsHgtFusion();
 
@@ -1087,19 +1122,11 @@ private:
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
 
-	// load and save mag field state covariance data for re-use
-	void loadMagCovData();
-	void saveMagCovData();
-
 	void resetGyroBiasZCov();
 
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatFromOtherStates();
 
-	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
-	// sensor measurement
-	float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
-
 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
 	{
 		return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -234,7 +234,10 @@ void Ekf::constrainStates()
 	_state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit);
 
 	_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
 }
 
@@ -671,6 +674,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	// return the largest magnetometer innovation test ratio
 	mag = 0.f;
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg) {
 		mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
 	}
@@ -678,6 +682,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	if (_control_status.flags.mag_3D) {
 		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 	if (_control_status.flags.gps_yaw) {
@@ -791,6 +796,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 
 	bool mag_innov_good = true;
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg) {
 		if (_aid_src_mag_heading.test_ratio < 1.f) {
 			mag_innov_good = false;
@@ -801,6 +807,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 			mag_innov_good = false;
 		}
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	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;
@@ -945,26 +952,6 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 	P.slice<4, 4>(0, 0) += q_cov;
 }
 
-void Ekf::saveMagCovData()
-{
-	// save the NED axis covariance sub-matrix
-	_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
-
-	// save the XYZ body covariance sub-matrix
-	_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
-}
-
-void Ekf::loadMagCovData()
-{
-	// re-instate the NED axis covariance sub-matrix
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
-	P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
-
-	// re-instate the XYZ body axis covariance sub-matrix
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
-	P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
-}
-
 void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -47,7 +47,9 @@
 EstimatorInterface::~EstimatorInterface()
 {
 	delete _gps_buffer;
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	delete _mag_buffer;
+#endif // CONFIG_EKF2_MAGNETOMETER
 	delete _baro_buffer;
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	delete _range_buffer;
@@ -102,6 +104,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 #endif // CONFIG_EKF2_DRAG_FUSION
 }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
 	if (!_initialised) {
@@ -137,6 +140,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 void EstimatorInterface::setGpsData(const gpsMessage &gps)
 {
@@ -707,9 +711,11 @@ void EstimatorInterface::print_status()
 		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
 	}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_mag_buffer) {
 		printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	if (_baro_buffer) {
 		printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());

src/modules/ekf2/EKF/estimator_interface.h

@@ -86,7 +86,9 @@ public:
 
 	void setIMUData(const imuSample &imu_sample);
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	void setMagData(const magSample &mag_sample);
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	void setGpsData(const gpsMessage &gps);
 
@@ -231,6 +233,7 @@ public:
 	Vector3f getPosition() const { return _output_predictor.getPosition(); }
 	const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
 	// At the next startup, set param.mag_declination_deg to the value saved
@@ -263,6 +266,7 @@ public:
 		strength_gs = _mag_strength;
 		strength_ref_gs = _mag_strength_gps;
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	// get EKF mode status
 	const filter_control_status_u &control_status() const { return _control_status; }
@@ -412,7 +416,12 @@ protected:
 	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	RingBuffer<magSample> *_mag_buffer{nullptr};
+	uint64_t _time_last_mag_buffer_push{0};
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
 
 #if defined(CONFIG_EKF2_AIRSPEED)
@@ -429,7 +438,6 @@ protected:
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
 	uint64_t _time_last_gps_buffer_push{0};
-	uint64_t _time_last_mag_buffer_push{0};
 	uint64_t _time_last_baro_buffer_push{0};
 
 	uint64_t _time_last_gnd_effect_on{0};

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -158,7 +158,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 
 			} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
 				// turn on fusion of external vision yaw measurements and disable all other heading fusion
-				stopMagFusion();
 				stopGpsYawFusion();
 				stopGpsFusion();
 

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -217,3 +217,22 @@ void Ekf::stopMagFusion()
 	}
 }
 
+void Ekf::saveMagCovData()
+{
+	// save the NED axis covariance sub-matrix
+	_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
+
+	// save the XYZ body covariance sub-matrix
+	_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
+}
+
+void Ekf::loadMagCovData()
+{
+	// re-instate the NED axis covariance sub-matrix
+	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
+	P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
+
+	// re-instate the XYZ body axis covariance sub-matrix
+	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
+	P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
+}

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -254,114 +254,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 	return false;
 }
 
-// update quaternion states and covariances using the yaw innovation and yaw observation variance
-bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
-{
-	Vector24f H_YAW;
-	computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
-
-	return fuseYaw(aid_src_status, H_YAW);
-}
-
-bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
-{
-	// define the innovation gate size
-	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
-
-	// innovation test ratio
-	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
-
-	if (aid_src_status.fusion_enabled) {
-
-		// check if the innovation variance calculation is badly conditioned
-		if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
-			// the innovation variance contribution from the state covariances is not negative, no fault
-			_fault_status.flags.bad_hdg = false;
-
-		} else {
-			// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-			_fault_status.flags.bad_hdg = true;
-
-			// we reinitialise the covariance matrix and abort this fusion step
-			initialiseCovariance();
-			ECL_ERR("yaw fusion numerical error - covariance reset");
-
-			return false;
-		}
-
-		// calculate the Kalman gains
-		// only calculate gains for states we are using
-		Vector24f Kfusion;
-		const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
-
-		for (uint8_t row = 0; row < _k_num_states; row++) {
-			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row, col) * H_YAW(col);
-			}
-
-			Kfusion(row) *= heading_innov_var_inv;
-		}
-
-		// set the magnetometer unhealthy if the test fails
-		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
-			// we allow to use it when on the ground because the large innovation could be caused
-			// by interference or a large initial gyro bias
-			if (!_control_status.flags.in_air
-			    && isTimedOut(_time_last_in_air, (uint64_t)5e6)
-			    && 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) * aid_src_status.innovation_variance));
-				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
-				resetGyroBiasZCov();
-
-			} else {
-				return false;
-			}
-
-		} else {
-			_innov_check_fail_status.flags.reject_yaw = false;
-		}
-
-		if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
-
-			_time_last_heading_fuse = _time_delayed_us;
-
-			aid_src_status.time_last_fuse = _time_delayed_us;
-			aid_src_status.fused = true;
-
-			_fault_status.flags.bad_hdg = false;
-
-			return true;
-
-		} else {
-			_fault_status.flags.bad_hdg = true;
-		}
-	}
-
-	// otherwise
-	aid_src_status.fused = false;
-	return false;
-}
-
-void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
-{
-	if (shouldUse321RotationSequence(_R_to_earth)) {
-		sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
-
-	} else {
-		sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
-	}
-}
-
 bool Ekf::fuseDeclination(float decl_sigma)
 {
 	if (!_control_status.flags.mag) {

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+
+#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
+#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
+
+#include <mathlib/mathlib.h>
+
+// update quaternion states and covariances using the yaw innovation and yaw observation variance
+bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
+{
+	Vector24f H_YAW;
+	computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
+
+	return fuseYaw(aid_src_status, H_YAW);
+}
+
+bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
+{
+	// define the innovation gate size
+	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
+
+	// innovation test ratio
+	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
+
+	if (aid_src_status.fusion_enabled) {
+
+		// check if the innovation variance calculation is badly conditioned
+		if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
+			// the innovation variance contribution from the state covariances is not negative, no fault
+			_fault_status.flags.bad_hdg = false;
+
+		} else {
+			// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+			_fault_status.flags.bad_hdg = true;
+
+			// we reinitialise the covariance matrix and abort this fusion step
+			initialiseCovariance();
+			ECL_ERR("yaw fusion numerical error - covariance reset");
+
+			return false;
+		}
+
+		// calculate the Kalman gains
+		// only calculate gains for states we are using
+		Vector24f Kfusion;
+		const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
+
+		for (uint8_t row = 0; row < _k_num_states; row++) {
+			for (uint8_t col = 0; col <= 3; col++) {
+				Kfusion(row) += P(row, col) * H_YAW(col);
+			}
+
+			Kfusion(row) *= heading_innov_var_inv;
+		}
+
+		// set the magnetometer unhealthy if the test fails
+		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
+			// we allow to use it when on the ground because the large innovation could be caused
+			// by interference or a large initial gyro bias
+			if (!_control_status.flags.in_air
+			    && isTimedOut(_time_last_in_air, (uint64_t)5e6)
+			    && 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) * aid_src_status.innovation_variance));
+				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
+				resetGyroBiasZCov();
+
+			} else {
+				return false;
+			}
+
+		} else {
+			_innov_check_fail_status.flags.reject_yaw = false;
+		}
+
+		if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
+
+			_time_last_heading_fuse = _time_delayed_us;
+
+			aid_src_status.time_last_fuse = _time_delayed_us;
+			aid_src_status.fused = true;
+
+			_fault_status.flags.bad_hdg = false;
+
+			return true;
+
+		} else {
+			_fault_status.flags.bad_hdg = true;
+		}
+	}
+
+	// otherwise
+	aid_src_status.fused = false;
+	return false;
+}
+
+void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
+{
+	if (shouldUse321RotationSequence(_R_to_earth)) {
+		sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
+
+	} else {
+		sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
+	}
+}