ekf2: separate mag and mag heading control logic (#21212)

- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d)
 - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion
 - if WMM available use for mag_I and mag_B init
 - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc)
 - mag reset if declination changed (eliminate _mag_yaw_reset_req)
 - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north)

Co-authored-by: bresch <brescianimathieu@gmail.com>

msg/EstimatorStatusFlags.msg

@@ -39,6 +39,8 @@ bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic cons
 bool cs_fake_pos                # 32 - true when fake position measurements are being fused
 bool cs_fake_hgt                # 33 - true when fake height measurements are being fused
 bool cs_gravity_vector          # 34 - true when gravity vector measurements are being fused
+bool cs_mag                     # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
+bool cs_ev_yaw_fault            # 36 - true when the EV heading has been declared faulty and is no longer being used
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/CMakeLists.txt

@@ -84,8 +84,10 @@ list(APPEND EKF_SRCS
 	EKF/gravity_fusion.cpp
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
+	EKF/mag_3d_control.cpp
 	EKF/mag_control.cpp
 	EKF/mag_fusion.cpp
+	EKF/mag_heading_control.cpp
 	EKF/output_predictor.cpp
 	EKF/vel_pos_fusion.cpp
 	EKF/zero_innovation_heading_update.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2018 ECL Development Team. All rights reserved.
+#   Copyright (c) 2015-2023 ECL Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -49,8 +49,10 @@ 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
 	zero_innovation_heading_update.cpp

src/modules/ekf2/EKF/common.h

@@ -102,9 +102,6 @@ enum MagFuseType : uint8_t {
 	// Integer definitions for mag_fusion_type
 	AUTO    = 0,   	///< The selection of either heading or 3D magnetometer fusion will be automatic
 	HEADING = 1,   	///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
-	MAG_3D  = 2,   	///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
-	UNUSED  = 3,    ///< Not implemented
-	INDOOR  = 4,   	///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
 	NONE    = 5    	///< Do not use magnetometer under any circumstance..
 };
 
@@ -335,7 +332,7 @@ struct parameters {
 
 	// magnetometer fusion
 	float mag_heading_noise{3.0e-1f};       ///< measurement noise used for simple heading fusion (rad)
-	float mag_noise{5.0e-2f};               ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
+	float mag_noise{5.0e-2f};               ///< measurement noise used for 3-axis magnetometer fusion (Gauss)
 	float mag_declination_deg{0.0f};        ///< magnetic declination (degrees)
 	float heading_innov_gate{2.6f};         ///< heading fusion innovation consistency gate size (STD)
 	float mag_innov_gate{3.0f};             ///< magnetometer fusion innovation consistency gate size (STD)
@@ -601,6 +598,9 @@ union filter_control_status_u {
 		uint64_t fake_pos                : 1; ///< 32 - true when fake position measurements are being fused
 		uint64_t fake_hgt                : 1; ///< 33 - true when fake height measurements are being fused
 		uint64_t gravity_vector          : 1; ///< 34 - true when gravity vector measurements are being fused
+		uint64_t mag                     : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
+		uint64_t ev_yaw_fault            : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
+
 	} flags;
 	uint64_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -76,7 +76,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 			_control_status.flags.tilt_align = true;
 
 			// send alignment status message to the console
-			const char *height_source = nullptr;
+			const char *height_source = "unknown";
 
 			if (_control_status.flags.baro_hgt) {
 				height_source = "baro";
@@ -89,16 +89,16 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 			} else if (_control_status.flags.rng_hgt) {
 				height_source = "rng";
-
-			} else {
-				height_source = "unknown";
-
 			}
 
-			if (height_source) {
-				ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
+			ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
 					 (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
-			}
+
+			ECL_DEBUG("tilt aligned, roll: %.3f, pitch %.3f, yaw: %.3f",
+				  (double)matrix::Eulerf(_state.quat_nominal).phi(),
+				  (double)matrix::Eulerf(_state.quat_nominal).theta(),
+				  (double)matrix::Eulerf(_state.quat_nominal).psi()
+				 );
 		}
 	}
 

src/modules/ekf2/EKF/covariance.cpp

@@ -180,7 +180,7 @@ 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;
 
-	if (_control_status.flags.mag_3D && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
+	if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
 		mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
 
 	} else {
@@ -190,7 +190,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	// 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;
 
-	if (_control_status.flags.mag_3D && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
+	if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
 		mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
 
 	} else {
@@ -292,7 +292,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		P(row, row) = nextP(row, row);
 	}
 
-	if (_control_status.flags.mag_3D) {
+	if (_control_status.flags.mag) {
 		for (unsigned row = 16; row <= 21; row++) {
 			for (unsigned column = 0 ; column < row; column++) {
 				P(row, column) = P(column, row) = nextP(column, row);
@@ -471,8 +471,9 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	}
 
 	// magnetic field states
-	if (!_control_status.flags.mag_3D) {
-		zeroMagCov();
+	if (!_control_status.flags.mag) {
+		P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
+		P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
 
 	} else {
 		// constrain variances
@@ -525,12 +526,6 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
 	return healthy;
 }
 
-void Ekf::resetMagRelatedCovariances()
-{
-	resetQuatCov();
-	resetMagCov();
-}
-
 void Ekf::resetQuatCov()
 {
 	zeroQuatCov();
@@ -542,7 +537,7 @@ void Ekf::resetQuatCov()
 	initialiseQuatCovariances(rot_vec_var);
 
 	// update the yaw angle variance using the variance of the measurement
-	if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
+	if (_params.mag_fusion_type != MagFuseType::NONE) {
 		// using magnetic heading tuning parameter
 		increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
 	}
@@ -556,31 +551,15 @@ void Ekf::zeroQuatCov()
 
 void Ekf::resetMagCov()
 {
-	// reset the corresponding rows and columns in the covariance matrix and
-	// set the variances on the magnetic field states to the measurement variance
-	clearMagCov();
+	if (_mag_decl_cov_reset) {
+		ECL_INFO("reset mag covariance");
+		_mag_decl_cov_reset = false;
+	}
 
 	P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise));
 	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
 
-	if (!_control_status.flags.mag_3D) {
-		// save covariance data for re-use when auto-switching between heading and 3-axis fusion
-		// if already in 3-axis fusion mode, the covariances are automatically saved when switching out
-		// of this mode
-		saveMagCovData();
-	}
-}
-
-void Ekf::clearMagCov()
-{
-	zeroMagCov();
-	_mag_decl_cov_reset = false;
-}
-
-void Ekf::zeroMagCov()
-{
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
+	saveMagCovData();
 }
 
 void Ekf::resetGyroBiasZCov()

src/modules/ekf2/EKF/ekf.cpp

@@ -45,9 +45,12 @@
 
 bool Ekf::init(uint64_t timestamp)
 {
-	bool ret = initialise_interface(timestamp);
-	reset();
-	return ret;
+	if (!_initialised) {
+		_initialised = initialise_interface(timestamp);
+		reset();
+	}
+
+	return _initialised;
 }
 
 void Ekf::reset()

src/modules/ekf2/EKF/ekf.h

@@ -161,9 +161,8 @@ public:
 		if (_control_status.flags.mag_hdg) {
 			heading_innov = _aid_src_mag_heading.innovation;
 			return;
-		}
 
-		if (_control_status.flags.mag_3D) {
+		} else if (_control_status.flags.mag_3D) {
 			heading_innov = Vector3f(_aid_src_mag.innovation).max();
 			return;
 		}
@@ -188,9 +187,8 @@ public:
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_var = _aid_src_mag_heading.innovation_variance;
 			return;
-		}
 
-		if (_control_status.flags.mag_3D) {
+		} else if (_control_status.flags.mag_3D) {
 			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
 			return;
 		}
@@ -215,9 +213,8 @@ public:
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
 			return;
-		}
 
-		if (_control_status.flags.mag_3D) {
+		} else if (_control_status.flags.mag_3D) {
 			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
 			return;
 		}
@@ -378,11 +375,11 @@ public:
 	const Vector3f &getMagBias() const { return _state.mag_B; }
 	Vector3f getMagBiasVariance() const
 	{
-		if (_control_status.flags.mag_3D) {
+		if (_control_status.flags.mag) {
 			return Vector3f{P(19, 19), P(20, 20), P(21, 21)};
 		}
 
-		return _saved_mag_bf_variance;
+		return _saved_mag_bf_covmat.diag();
 	}
 	float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
 
@@ -547,6 +544,9 @@ 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
 
@@ -580,11 +580,10 @@ private:
 	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
-	uint64_t _mag_use_not_inhibit_us{0};	///< last system time in usec before magnetometer use was inhibited
-
-	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
+	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.
-	bool _synthetic_mag_z_active{false};	///< true if we are generating synthetic magnetometer Z measurements
+	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
@@ -657,7 +656,6 @@ private:
 	uint8_t _nb_ev_vel_reset_available{0};
 	uint8_t _nb_ev_yaw_reset_available{0};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
 
 	estimator_aid_source1d_s _aid_src_gnss_hgt{};
 	estimator_aid_source2d_s _aid_src_gnss_pos{};
@@ -706,9 +704,8 @@ private:
 	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};
-	Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
-	Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
-	float _saved_mag_ef_d_variance{};   ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
+	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)
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
@@ -765,8 +762,6 @@ private:
 
 	void updateGpsYaw(const gpsSample &gps_sample);
 
-	void startGpsYawFusion(const gpsSample &gps_sample);
-
 #endif // CONFIG_EKF2_GNSS_YAW
 	void stopGpsYawFusion();
 
@@ -922,14 +917,14 @@ private:
 		}
 
 		// mag I: states 16, 17, 18
-		if (!_control_status.flags.mag_3D) {
+		if (!_control_status.flags.mag) {
 			K(16) = 0.f;
 			K(17) = 0.f;
 			K(18) = 0.f;
 		}
 
 		// mag B: states 19, 20, 21
-		if (!_control_status.flags.mag_3D) {
+		if (!_control_status.flags.mag) {
 			K(19) = 0.f;
 			K(20) = 0.f;
 			K(21) = 0.f;
@@ -1022,23 +1017,20 @@ private:
 
 	// 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);
+	void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src);
 
-	bool resetMagHeading(const Vector3f &mag);
-	bool resetMagStates(const Vector3f &mag);
+	bool checkHaglYawResetReq() const;
+
+	void resetMagHeading(const Vector3f &mag);
+	void resetMagStates(const Vector3f &mag, bool reset_heading = true);
 	bool haglYawResetReq();
 
-	void selectMagAuto();
-	void check3DMagFusionSuitability();
 	void checkYawAngleObservability();
 	void checkMagBiasObservability();
-	bool canUse3DMagFusion() const;
 
-	void checkMagDeclRequired();
-	bool shouldInhibitMag() const;
 	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
-	void runMagAndMagDeclFusions(const Vector3f &mag);
-	void run3DMagAndDeclFusions(const Vector3f &mag);
 
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
@@ -1069,11 +1061,8 @@ private:
 	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
 
-	void stopMagFusion();
-	void stopMag3DFusion();
 	void stopMagHdgFusion();
-	void startMagHdgFusion();
-	void startMag3DFusion();
+	void stopMagFusion();
 
 	void stopBaroHgtFusion();
 	void stopGpsHgtFusion();
@@ -1087,11 +1076,9 @@ private:
 	// do not call before quaternion states are initialised
 	void initialiseQuatCovariances(Vector3f &rot_vec_var);
 
-	// perform a limited reset of the magnetic field related state covariances
-	void resetMagRelatedCovariances();
-
 	void resetQuatCov();
 	void zeroQuatCov();
+
 	void resetMagCov();
 
 	// perform a reset of the wind states and related covariances
@@ -1105,8 +1092,6 @@ private:
 	// load and save mag field state covariance data for re-use
 	void loadMagCovData();
 	void saveMagCovData();
-	void clearMagCov();
-	void zeroMagCov();
 
 	void resetGyroBiasZCov();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1086,33 +1086,24 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 	P(3,2) -= yaw_variance*SQ[0]*SQ[3];
 }
 
-// save covariance data for re-use when auto-switching between heading and 3-axis fusion
 void Ekf::saveMagCovData()
 {
-	// save variances for XYZ body axis field
-	_saved_mag_bf_variance(0) = P(19, 19);
-	_saved_mag_bf_variance(1) = P(20, 20);
-	_saved_mag_bf_variance(2) = P(21, 21);
+	// save the NED axis covariance sub-matrix
+	_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
 
-	// save the NE axis covariance sub-matrix
-	_saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16);
-
-	// save variance for the D earth axis
-	_saved_mag_ef_d_variance = P(18, 18);
+	// save the XYZ body covariance sub-matrix
+	_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
 }
 
 void Ekf::loadMagCovData()
 {
-	// re-instate variances for the XYZ body axis field
-	P(19, 19) = _saved_mag_bf_variance(0);
-	P(20, 20) = _saved_mag_bf_variance(1);
-	P(21, 21) = _saved_mag_bf_variance(2);
+	// 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 NE axis covariance sub-matrix
-	P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat;
-
-	// re-instate the D earth axis variance
-	P(18, 18) = _saved_mag_ef_d_variance;
+	// 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)
@@ -1133,7 +1124,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	uncorrelateQuatFromOtherStates();
 
 	// update the yaw angle variance
-	if (yaw_variance > FLT_EPSILON) {
+	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
 		increaseQuatYawErrVariance(yaw_variance);
 	}
 
@@ -1182,8 +1173,6 @@ bool Ekf::resetYawToEKFGSF()
 
 	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
-	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
-	_flt_mag_align_start_time = _time_delayed_us;
 	_control_status.flags.yaw_align = true;
 	_information_events.flags.yaw_aligned_to_imu_gps = true;
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -471,7 +471,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 		_system_flag_buffer->push(system_flags_new);
 
 	} else {
-		ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
+		ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -140,16 +140,34 @@ public:
 	void set_in_air_status(bool in_air)
 	{
 		if (!in_air) {
+			if (_control_status.flags.in_air) {
+				ECL_DEBUG("no longer in air");
+			}
+
 			_time_last_on_ground_us = _time_delayed_us;
 
 		} else {
+			if (!_control_status.flags.in_air) {
+				ECL_DEBUG("in air");
+			}
+
 			_time_last_in_air = _time_delayed_us;
 		}
 
 		_control_status.flags.in_air = in_air;
 	}
 
-	void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; }
+	void set_vehicle_at_rest(bool at_rest)
+	{
+		if (!_control_status.flags.vehicle_at_rest && at_rest) {
+			ECL_DEBUG("at rest");
+
+		} else if (_control_status.flags.vehicle_at_rest && !at_rest) {
+			ECL_DEBUG("no longer at rest");
+		}
+
+		_control_status.flags.vehicle_at_rest = at_rest;
+	}
 
 	// return true if the attitude is usable
 	bool attitude_valid() const { return _control_status.flags.tilt_align; }
@@ -420,9 +438,12 @@ protected:
 	// allocate data buffers and initialize interface variables
 	bool initialise_interface(uint64_t timestamp);
 
+	uint64_t _wmm_gps_time_last_checked{0};  // time WMM last checked
+	uint64_t _wmm_gps_time_last_set{0};      // time WMM last set
 	float _mag_declination_gps{NAN};         // magnetic declination returned by the geo library using the last valid GPS position (rad)
 	float _mag_inclination_gps{NAN};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
 	float _mag_strength_gps{NAN};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
+
 	float _mag_inclination{NAN};
 	float _mag_strength{NAN};
 

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -49,10 +49,14 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 	aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
 	aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
 
+	if (ev_reset) {
+		_control_status.flags.ev_yaw_fault = false;
+	}
+
 	// determine if we should use EV yaw aiding
 	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
 					     && _control_status.flags.tilt_align
-					     && !_inhibit_ev_yaw_use
+					     && !_control_status.flags.ev_yaw_fault
 					     && PX4_ISFINITE(aid_src.observation)
 					     && PX4_ISFINITE(aid_src.observation_variance);
 

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -84,6 +84,7 @@ void Ekf::controlFakePosFusion()
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
 
 				if (is_fusion_failing) {
+					ECL_WARN("fake position fusion failing, resetting");
 					resetFakePosFusion();
 				}
 

src/modules/ekf2/EKF/gps_checks.cpp

@@ -84,52 +84,53 @@ bool Ekf::collect_gps(const gpsMessage &gps)
 
 		_NED_origin_initialised = true;
 
-		_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));
-
-		const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
-
-		// set the magnetic field data returned by the geo library using the current GPS position
-		_mag_declination_gps = get_mag_declination_radians(lat, lon);
-		_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
-		_mag_strength_gps = get_mag_strength_gauss(lat, lon);
-
-		// request a reset of the yaw using the new declination
-		if ((_params.mag_fusion_type != MagFuseType::NONE)
-		     && !declination_was_valid) {
-			_mag_yaw_reset_req = true;
-		}
-
 		// save the horizontal and vertical position uncertainty of the origin
 		_gps_origin_eph = gps.eph;
 		_gps_origin_epv = gps.epv;
 
 		_information_events.flags.gps_checks_passed = true;
 		ECL_INFO("GPS checks passed");
+	}
 
-	} else if (!_NED_origin_initialised) {
-		// a rough 2D fix is still sufficient to lookup declination
-		if ((gps.fix_type >= 2) && (gps.eph < 1000)) {
+	if (isTimedOut(_wmm_gps_time_last_checked, 1e6)) {
+		// a rough 2D fix is sufficient to lookup declination
+		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.eph < 1000);
 
-			const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
+		if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
 
 			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
 			const double lat = gps.lat * 1.0e-7;
 			const double lon = gps.lon * 1.0e-7;
 
 			// set the magnetic field data returned by the geo library using the current GPS position
-			_mag_declination_gps = get_mag_declination_radians(lat, lon);
-			_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
-			_mag_strength_gps = get_mag_strength_gauss(lat, lon);
+			const float mag_declination_gps = get_mag_declination_radians(lat, lon);
+			const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
+			const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
 
-			// request mag yaw reset if there's a mag declination for the first time
-			if (_params.mag_fusion_type != MagFuseType::NONE) {
-				if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) {
-					_mag_yaw_reset_req = true;
+			if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
+
+				const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
+				const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
+
+				if ((_wmm_gps_time_last_set == 0)
+				    || !PX4_ISFINITE(mag_declination_gps)
+				    || !PX4_ISFINITE(mag_inclination_gps)
+				    || !PX4_ISFINITE(mag_strength_gps)
+				    || mag_declination_changed
+				    || mag_inclination_changed
+				   ) {
+					_mag_declination_gps = mag_declination_gps;
+					_mag_inclination_gps = mag_inclination_gps;
+					_mag_strength_gps = mag_strength_gps;
+
+					_wmm_gps_time_last_set = _time_delayed_us;
 				}
 			}
 
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
 		}
+
+		_wmm_gps_time_last_checked = _time_delayed_us;
 	}
 
 	// start collecting GPS if there is a 3D fix and the NED origin has been set

src/modules/ekf2/EKF/gps_control.cpp

@@ -41,7 +41,7 @@
 
 void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
-	if (!_gps_buffer || !((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
+	if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
 		stopGpsFusion();
 		return;
 	}
@@ -166,7 +166,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 							ECL_WARN("GPS emergency yaw reset");
 
 							if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-								// stop using the magnetometer in the main EKF otherwise it' fusion could drag the yaw around
+								// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
 								// and cause another navigation failure
 								_control_status.flags.mag_fault = true;
 								_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
@@ -184,7 +184,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 
 							if (_control_status.flags.ev_yaw) {
-								_inhibit_ev_yaw_use = true;
+								_control_status.flags.ev_yaw_fault = true;
 							}
 
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -369,11 +369,6 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 			if (starting_conditions_passing) {
 				// Try to activate GPS yaw fusion
 				if (resetYawToGps(gps_sample.yaw)) {
-
-					stopEvYawFusion();
-					stopMagHdgFusion();
-					stopMag3DFusion();
-
 					ECL_INFO("starting GPS yaw fusion");
 
 					_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
@@ -429,8 +424,4 @@ void Ekf::stopGpsFusion()
 
 	stopGpsHgtFusion();
 	stopGpsYawFusion();
-
-	// We do not need to know the true North anymore
-	// EV yaw can start again
-	_inhibit_ev_yaw_use = false;
 }

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -0,0 +1,218 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_3d_control.cpp
+ * Control functions for ekf mag 3D fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
+			     estimator_aid_source3d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "mag";
+
+	resetEstimatorAidStatus(aid_src);
+
+	const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse);
+
+	// determine if we should use mag fusion
+	bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
+					     && _control_status.flags.tilt_align
+					     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6))
+					     && mag_sample.mag.longerThan(0.f)
+					     && mag_sample.mag.isAllFinite();
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing;
+
+	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
+	// before they are used to constrain heading drift
+	_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
+				       && _control_status.flags.mag
+				       && _control_status.flags.mag_aligned_in_flight
+				       && !_control_status.flags.mag_fault
+				       && isRecent(aid_src.time_last_fuse, 500'000)
+				       && getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f))
+				       && !_control_status.flags.ev_yaw
+				       && !_control_status.flags.gps_yaw;
+
+	// TODO: allow clearing mag_fault if mag_3d is good?
+
+	if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
+		ECL_INFO("starting mag 3D fusion");
+
+	} else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) {
+		ECL_INFO("stopping mag 3D fusion");
+	}
+
+	// if we are using 3-axis magnetometer fusion, but without external NE aiding,
+	// then the declination must be fused as an observation to prevent long term heading drift
+	// fusing declination when gps aiding is available is optional, but recommended to prevent
+	// problem if the vehicle is static for extended periods of time
+	const bool mag_decl_user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);
+	const bool not_using_ne_aiding = !_control_status.flags.gps;
+	_control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || mag_decl_user_selected));
+
+	if (_control_status.flags.mag) {
+		aid_src.timestamp_sample = mag_sample.time_us;
+		aid_src.fusion_enabled = true;
+
+		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
+
+			if (mag_sample.reset || checkHaglYawResetReq()) {
+				ECL_INFO("reset to %s", AID_SRC_NAME);
+				resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
+				aid_src.time_last_fuse = _time_delayed_us;
+
+			} else {
+				if (!_mag_decl_cov_reset) {
+					// After any magnetic field covariance reset event the earth field state
+					// covariances need to be corrected to incorporate knowledge of the declination
+					// before fusing magnetometer data to prevent rapid rotation of the earth field
+					// states for the first few observations.
+					fuseDeclination(0.02f);
+					_mag_decl_cov_reset = true;
+					fuseMag(mag_sample.mag, aid_src, false);
+
+				} 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.
+					const bool update_all_states = _control_status.flags.mag_3D;
+					fuseMag(mag_sample.mag, aid_src, update_all_states);
+
+					if (_control_status.flags.mag_dec) {
+						fuseDeclination(0.5f);
+					}
+				}
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
+
+			if (is_fusion_failing) {
+				if (_nb_mag_3d_reset_available > 0) {
+					// Data seems good, attempt a reset (mag states only unless mag_3D currently active)
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
+					aid_src.time_last_fuse = _time_delayed_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_mag_3d_reset_available--;
+					}
+
+				} else if (starting_conditions_passing) {
+					// Data seems good, but previous reset did not fix the issue
+					// something else must be wrong, declare the sensor faulty and stop the fusion
+					//_control_status.flags.mag_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopMagFusion();
+
+				} else {
+					// A reset did not fix the issue but all the starting checks are not passing
+					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					stopMagFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME);
+			stopMagFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+
+			_control_status.flags.mag = true;
+
+			loadMagCovData();
+
+			// activate fusion, reset mag states and initialize variance if first init or in flight reset
+			if (!_control_status.flags.yaw_align
+			    || wmm_updated
+			    || !_mag_decl_cov_reset
+			    || !_state.mag_I.longerThan(0.f)
+			    || (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I
+			    || (P.slice<3, 3>(19, 19).diag().min() < sq(0.0001f)) // mag_B
+			   ) {
+				ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
+
+				bool reset_heading = !_control_status.flags.yaw_align;
+
+				resetMagStates(_mag_lpf.getState(), reset_heading);
+
+				if (reset_heading) {
+					_control_status.flags.yaw_align = true;
+				}
+
+			} else {
+				ECL_INFO("starting %s fusion", AID_SRC_NAME);
+				fuseMag(mag_sample.mag, aid_src, false);
+			}
+
+			aid_src.time_last_fuse = _time_delayed_us;
+
+			_nb_mag_3d_reset_available = 2;
+		}
+	}
+
+	aid_src.fusion_enabled = _control_status.flags.mag;
+}
+
+void Ekf::stopMagFusion()
+{
+	if (_control_status.flags.mag) {
+		ECL_INFO("stopping mag fusion");
+
+		_control_status.flags.mag = false;
+		_control_status.flags.mag_dec = false;
+
+		if (_control_status.flags.mag_3D) {
+			ECL_INFO("stopping mag 3D fusion");
+			_control_status.flags.mag_3D = false;
+		}
+
+		_fault_status.flags.bad_mag_x = false;
+		_fault_status.flags.bad_mag_y = false;
+		_fault_status.flags.bad_mag_z = false;
+
+		_fault_status.flags.bad_mag_decl = false;
+
+		saveMagCovData();
+	}
+}
+

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -54,10 +54,10 @@
 bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states)
 {
 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-	const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
+	const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
 
 	// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
-	const char* numerical_error_covariance_reset_string = "numerical error - covariance reset";
+	const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
 	Vector3f mag_innov;
 	Vector3f innov_var;
 
@@ -75,7 +75,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		_fault_status.flags.bad_mag_x = true;
 
 		// we need to re-initialise covariances and abort this fusion step
-		resetMagRelatedCovariances();
+		if (update_all_states) {
+			resetQuatCov();
+		}
+
+		resetMagCov();
+
 		ECL_ERR("magX %s", numerical_error_covariance_reset_string);
 		return false;
 	}
@@ -88,7 +93,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		_fault_status.flags.bad_mag_y = true;
 
 		// we need to re-initialise covariances and abort this fusion step
-		resetMagRelatedCovariances();
+		if (update_all_states) {
+			resetQuatCov();
+		}
+
+		resetMagCov();
+
 		ECL_ERR("magY %s", numerical_error_covariance_reset_string);
 		return false;
 	}
@@ -100,7 +110,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		_fault_status.flags.bad_mag_z = true;
 
 		// we need to re-initialise covariances and abort this fusion step
-		resetMagRelatedCovariances();
+		if (update_all_states) {
+			resetQuatCov();
+		}
+
+		resetMagCov();
+
 		ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
 		return false;
 	}
@@ -118,7 +133,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		aid_src_mag.innovation[i] = mag_innov(i);
 	}
 
-	aid_src_mag.fusion_enabled = _control_status.flags.mag_3D && update_all_states;
+	aid_src_mag.fusion_enabled = _control_status.flags.mag;
 
 	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
 	if (_control_status.flags.synthetic_mag_z) {
@@ -159,7 +174,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 				_fault_status.flags.bad_mag_y = true;
 
 				// we need to re-initialise covariances and abort this fusion step
-				resetMagRelatedCovariances();
+				if (update_all_states) {
+					resetQuatCov();
+				}
+
+				resetMagCov();
+
 				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
 				return false;
 			}
@@ -183,7 +203,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 				_fault_status.flags.bad_mag_z = true;
 
 				// we need to re-initialise covariances and abort this fusion step
-				resetMagRelatedCovariances();
+				if (update_all_states) {
+					resetQuatCov();
+				}
+
+				resetMagCov();
+
 				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
 				return false;
 			}
@@ -214,7 +239,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 	_fault_status.flags.bad_mag_y = !fused[1];
 	_fault_status.flags.bad_mag_z = !fused[2];
 
-	if (fused[0] && fused[1] && fused[2]) {
+	if (fused[0] && fused[1] && (fused[2] || _control_status.flags.synthetic_mag_z)) {
 		aid_src_mag.fused = true;
 		aid_src_mag.time_last_fuse = _time_delayed_us;
 
@@ -230,7 +255,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 }
 
 // update quaternion states and covariances using the yaw innovation and yaw observation variance
-bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status)
+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);
@@ -238,7 +263,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status)
 	return fuseYaw(aid_src_status, H_YAW);
 }
 
-bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status, const Vector24f &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);
@@ -285,9 +310,9 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status, const Vector24f &H_Y
 			// 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)
-			) {
+			    && 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
@@ -339,6 +364,10 @@ void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vec
 
 bool Ekf::fuseDeclination(float decl_sigma)
 {
+	if (!_control_status.flags.mag) {
+		return false;
+	}
+
 	// observation variance (rad**2)
 	const float R_DECL = sq(decl_sigma);
 
@@ -346,6 +375,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
 	float decl_pred;
 	float innovation_variance;
 
+	// TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter?
 	sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
 
 	const float innovation = wrap_pi(decl_pred - getMagDeclination());
@@ -373,26 +403,29 @@ bool Ekf::fuseDeclination(float decl_sigma)
 
 void Ekf::limitDeclination()
 {
+	const Vector3f mag_I_before = _state.mag_I;
+
 	// get a reference value for the earth field declinaton and minimum plausible horizontal field strength
-	// set to 50% of the horizontal strength from geo tables if location is known
-	float decl_reference;
+	float decl_reference = NAN;
 	float h_field_min = 0.001f;
 
-	if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
-		// use parameter value until GPS is available, then use value returned by geo library
-		if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
-			decl_reference = _mag_declination_gps;
-			h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
+	if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL
+	    && (PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps) && PX4_ISFINITE(_mag_inclination_gps))
+	   ) {
+		decl_reference = _mag_declination_gps;
 
-		} else {
-			decl_reference = math::radians(_params.mag_declination_deg);
-		}
+		// set to 50% of the horizontal strength from geo tables if location is known
+		h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
 
-	} else {
-		// always use the parameter value
+	} else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) {
+		// use parameter value if GPS isn't available
 		decl_reference = math::radians(_params.mag_declination_deg);
 	}
 
+	if (!PX4_ISFINITE(decl_reference)) {
+		return;
+	}
+
 	// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
 	// and can result in a reversal of the NE field states which the filter cannot recover from
 	// apply a circular limit
@@ -406,9 +439,8 @@ void Ekf::limitDeclination()
 
 		} else {
 			// too small to scale radially so set to expected value
-			const float mag_declination = getMagDeclination();
-			_state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination);
-			_state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination);
+			_state.mag_I(0) = 2.0f * h_field_min * cosf(decl_reference);
+			_state.mag_I(1) = 2.0f * h_field_min * sinf(decl_reference);
 		}
 
 		h_field = h_field_min;
@@ -428,6 +460,14 @@ void Ekf::limitDeclination()
 		_state.mag_I(0) = h_field * cosf(decl_min);
 		_state.mag_I(1) = h_field * sinf(decl_min);
 	}
+
+	if ((mag_I_before - _state.mag_I).longerThan(0.01f)) {
+		ECL_DEBUG("declination limited mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f] (decl: %.3f)",
+			  (double)mag_I_before(0), (double)mag_I_before(1), (double)mag_I_before(2),
+			  (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
+			  (double)decl_reference
+			 );
+	}
 }
 
 float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)

src/modules/ekf2/EKF/mag_heading_control.cpp

@@ -0,0 +1,190 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_heading_control.cpp
+ * Control functions for ekf mag heading fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
+				  estimator_aid_source1d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "mag heading";
+
+	// use mag bias if variance good
+	Vector3f mag_bias{0.f, 0.f, 0.f};
+	const Vector3f mag_bias_var = getMagBiasVariance();
+
+	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
+		mag_bias = _state.mag_B;
+	}
+
+	// calculate mag heading
+	// Rotate the measurements into earth frame using the zero yaw angle
+	const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
+
+	// the angle of the projection onto the horizontal gives the yaw angle
+	// calculate the yaw innovation and wrap to the interval between +-pi
+	const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias);
+	const float declination = getMagDeclination();
+	const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
+
+	resetEstimatorAidStatus(aid_src);
+	aid_src.observation = measured_hdg;
+	aid_src.observation_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
+
+	if (_control_status.flags.yaw_align) {
+		// mag heading
+		aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
+
+	} else {
+		// mag heading delta (logging only)
+		aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev)
+					     - wrap_pi(measured_hdg - _mag_heading_prev));
+	}
+
+	// determine if we should use mag heading aiding
+	bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::HEADING)
+					      || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
+					     && _control_status.flags.tilt_align
+					     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && !_control_status.flags.mag_fault
+					     && !_control_status.flags.mag_field_disturbed
+					     && !_control_status.flags.ev_yaw
+					     && !_control_status.flags.gps_yaw
+					     && PX4_ISFINITE(aid_src.observation)
+					     && PX4_ISFINITE(aid_src.observation_variance);
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing
+			&& isTimedOut(aid_src.time_last_fuse, 3e6);
+
+	if (_control_status.flags.mag_hdg) {
+		aid_src.fusion_enabled = true;
+		aid_src.timestamp_sample = mag_sample.time_us;
+
+		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
+
+			const bool declination_changed = _control_status_prev.flags.mag_hdg
+							 && (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f));
+
+			if (mag_sample.reset || declination_changed) {
+				if (declination_changed) {
+					ECL_INFO("reset to %s, declination changed %.5f->%.5f",
+						 AID_SRC_NAME, (double)_mag_heading_last_declination, (double)declination);
+
+				} else {
+					ECL_INFO("reset to %s", AID_SRC_NAME);
+				}
+
+				resetMagHeading(_mag_lpf.getState());
+				aid_src.time_last_fuse = _time_delayed_us;
+
+			} else {
+				fuseYaw(aid_src);
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
+
+			if (is_fusion_failing) {
+				if (_nb_mag_heading_reset_available > 0) {
+					// Data seems good, attempt a reset
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetMagHeading(_mag_lpf.getState());
+					aid_src.time_last_fuse = _time_delayed_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_mag_heading_reset_available--;
+					}
+
+				} else if (starting_conditions_passing) {
+					// Data seems good, but previous reset did not fix the issue
+					// something else must be wrong, declare the sensor faulty and stop the fusion
+					_control_status.flags.mag_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopMagHdgFusion();
+
+				} else {
+					// A reset did not fix the issue but all the starting checks are not passing
+					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					stopMagHdgFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
+			stopMagHdgFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+			// activate fusion
+			ECL_INFO("starting %s fusion", AID_SRC_NAME);
+
+			if (!_control_status.flags.yaw_align) {
+				// reset heading
+				resetMagHeading(_mag_lpf.getState());
+				_control_status.flags.yaw_align = true;
+			}
+
+			_control_status.flags.mag_hdg = true;
+			aid_src.time_last_fuse = _time_delayed_us;
+
+			_nb_mag_heading_reset_available = 1;
+		}
+	}
+
+	aid_src.fusion_enabled = _control_status.flags.mag_hdg;
+
+	// record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only)
+	_mag_heading_prev = measured_hdg;
+	_mag_heading_pred_prev = getEulerYaw(_state.quat_nominal);
+
+	_mag_heading_last_declination = getMagDeclination();
+}
+
+void Ekf::stopMagHdgFusion()
+{
+	if (_control_status.flags.mag_hdg) {
+		ECL_INFO("stopping mag heading fusion");
+		resetEstimatorAidStatus(_aid_src_mag_heading);
+
+		_control_status.flags.mag_hdg = false;
+
+		_fault_status.flags.bad_hdg = false;
+	}
+}

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -44,13 +44,12 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 				|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
 
 	// fuse zero innovation at a limited rate if the yaw variance is too large
-	if (_control_status.flags.tilt_align
-	    && !yaw_aiding
+	if(!yaw_aiding
 	    && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
 
 		// Use an observation variance larger than usual but small enough
 		// to constrain the yaw variance just below the threshold
-		const float obs_var = 0.25f;
+		const float obs_var = _control_status.flags.tilt_align ? 0.25f : 0.001f;
 
 		estimator_aid_source1d_s aid_src_status{};
 		aid_src_status.observation = getEulerYaw(_state.quat_nominal);
@@ -61,7 +60,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 
 		computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
 
-		if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
+		if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
 			// The yaw variance is too large, fuse fake measurement
 			aid_src_status.fusion_enabled = true;
 			fuseYaw(aid_src_status, H_YAW);

src/modules/ekf2/EKF/zero_velocity_update.cpp

@@ -46,7 +46,7 @@ void Ekf::controlZeroVelocityUpdate()
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
 				&& _control_status_prev.flags.vehicle_at_rest
-				&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift
+				&& (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift
 
 		if (continuing_conditions_passing) {
 			Vector3f vel_obs{0, 0, 0};

src/modules/ekf2/EKF2.cpp

@@ -312,6 +312,13 @@ bool EKF2::multi_init(int imu, int mag)
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+
+	// mag advertise
+	if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
+		_estimator_aid_src_mag_heading_pub.advertise();
+		_estimator_aid_src_mag_pub.advertise();
+	}
+
 	_attitude_pub.advertise();
 	_local_position_pub.advertise();
 	_global_position_pub.advertise();
@@ -910,6 +917,24 @@ void EKF2::VerifyParams()
 	}
 
 #endif // CONFIG_EKF2_OPTICAL_FLOW
+
+
+	// EKF2_MAG_TYPE obsolete options
+	if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO)
+	    && (_param_ekf2_mag_type.get() != MagFuseType::HEADING)
+	    && (_param_ekf2_mag_type.get() != MagFuseType::NONE)
+	   ) {
+
+		mavlink_log_critical(&_mavlink_log_pub, "EKF2_MAG_TYPE invalid, resetting to default");
+		/* EVENT
+		 * @description <param>EKF2_AID_MASK</param> is set to {1:.0}.
+		 */
+		events::send<float>(events::ID("ekf2_mag_type_invalid"), events::Log::Warning,
+				    "EKF2_MAG_TYPE invalid, resetting to default", _param_ekf2_mag_type.get());
+
+		_param_ekf2_mag_type.set(0);
+		_param_ekf2_mag_type.commit();
+	}
 }
 
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
@@ -1570,31 +1595,37 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 
 		// take device ids from sensor_selection_s if not using specific vehicle_imu_s
 		if ((_device_id_gyro != 0) && (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::GyroBias))) {
+			const Vector3f bias_var{_ekf.getGyroBiasVariance()};
+
 			bias.gyro_device_id = _device_id_gyro;
 			gyro_bias.copyTo(bias.gyro_bias);
 			bias.gyro_bias_limit = _ekf.getGyroBiasLimit();
-			_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
-			bias.gyro_bias_valid = true;  // TODO
+			bias_var.copyTo(bias.gyro_bias_variance);
+			bias.gyro_bias_valid = bias_var.longerThan(0.f) && !bias_var.longerThan(0.1f);
 			bias.gyro_bias_stable = _gyro_cal.cal_available;
 			_last_gyro_bias_published = gyro_bias;
 		}
 
 		if ((_device_id_accel != 0) && (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::AccelBias))) {
+			const Vector3f bias_var{_ekf.getAccelBiasVariance()};
+
 			bias.accel_device_id = _device_id_accel;
 			accel_bias.copyTo(bias.accel_bias);
 			bias.accel_bias_limit = _ekf.getAccelBiasLimit();
-			_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
-			bias.accel_bias_valid = true;  // TODO
+			bias_var.copyTo(bias.accel_bias_variance);
+			bias.accel_bias_valid = bias_var.longerThan(0.f) && !bias_var.longerThan(0.1f);
 			bias.accel_bias_stable = _accel_cal.cal_available;
 			_last_accel_bias_published = accel_bias;
 		}
 
 		if (_device_id_mag != 0) {
+			const Vector3f bias_var{_ekf.getMagBiasVariance()};
+
 			bias.mag_device_id = _device_id_mag;
 			mag_bias.copyTo(bias.mag_bias);
 			bias.mag_bias_limit = _ekf.getMagBiasLimit();
-			_ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance);
-			bias.mag_bias_valid = true; // TODO
+			bias_var.copyTo(bias.mag_bias_variance);
+			bias.mag_bias_valid = bias_var.longerThan(0.f) && !bias_var.longerThan(0.1f);
 			bias.mag_bias_stable = _mag_cal.cal_available;
 			_last_mag_bias_published = mag_bias;
 		}
@@ -1738,6 +1769,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_fake_pos                = _ekf.control_status_flags().fake_pos;
 		status_flags.cs_fake_hgt                = _ekf.control_status_flags().fake_hgt;
 		status_flags.cs_gravity_vector          = _ekf.control_status_flags().gravity_vector;
+		status_flags.cs_mag                     = _ekf.control_status_flags().mag;
+		status_flags.cs_ev_yaw_fault            = _ekf.control_status_flags().ev_yaw_fault;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
@@ -2523,14 +2556,16 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
-	const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D)
-				&& !_ekf.control_status_flags().mag_fault
-				&& !_ekf.control_status_flags().mag_field_disturbed;
+	const Vector3f mag_bias = _ekf.getMagBias();
+	const Vector3f mag_bias_var = _ekf.getMagBiasVariance();
 
-	const bool learning_valid = bias_valid && _ekf.control_status_flags().mag_3D;
+	const bool bias_valid = (_ekf.fault_status().value == 0)
+				&& _ekf.control_status_flags().yaw_align
+				&& mag_bias_var.longerThan(0.f) && !mag_bias_var.longerThan(0.02f);
 
-	UpdateCalibration(timestamp, _mag_cal, _ekf.getMagBias(), _ekf.getMagBiasVariance(), _ekf.getMagBiasLimit(),
-			  bias_valid, learning_valid);
+	const bool learning_valid = bias_valid && _ekf.control_status_flags().mag;
+
+	UpdateCalibration(timestamp, _mag_cal, mag_bias, mag_bias_var, _ekf.getMagBiasLimit(), bias_valid, learning_valid);
 
 	// update stored declination value
 	if (!_mag_decl_saved) {

src/modules/ekf2/ekf2_params.c

@@ -493,16 +493,10 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
  * The fusion of magnetometer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field.
  * If set to 'Automatic' magnetic heading fusion is used when on-ground and 3-axis magnetic field fusion in-flight with fallback to magnetic heading fusion if there is insufficient motion to make yaw or magnetic field states observable.
  * If set to 'Magnetic heading' magnetic heading fusion is used at all times.
- * If set to '3-axis' 3-axis field fusion is used at all times.
- * If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight.
- * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
  * If set to 'None' the magnetometer will not be used under any circumstance. If no external source of yaw is available, it is possible to use post-takeoff horizontal movement combined with GPS velocity measurements to align the yaw angle with the timer required (depending on the amount of movement and GPS data quality).
  * @group EKF2
  * @value 0 Automatic
  * @value 1 Magnetic heading
- * @value 2 3-axis
- * @value 3 VTOL custom
- * @value 4 MC custom
  * @value 5 None
  * @reboot_required true
  */

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -50,12 +50,15 @@ public:
 	SensorSimulator _sensor_simulator;
 	EkfWrapper _ekf_wrapper;
 
-	const float _init_tilt_period = 0.3f; // seconds
+	const float _init_tilt_period = 0.7f; // seconds
 
-	// GTests is calling this
+	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
+		// first init, then manually set in air and at rest (default for a real vehicle)
 		_ekf->init(0);
+		_ekf->set_in_air_status(false);
+		_ekf->set_vehicle_at_rest(true);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -66,28 +69,19 @@ public:
 	void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion)
 	{
 		const Quatf quat_est = _ekf->getQuaternion();
-		const float precision = 0.0002f; // TODO: this is only required for the pitch90 test to pass
-		EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion, precision))
+		EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion))
 				<< "quat est = " << quat_est(0) << ", " << quat_est(1) << ", "
 				<< quat_est(2) << ", " << quat_est(3)
 				<< "\nquat true = " << true_quaternion(0) << ", " << true_quaternion(1) << ", "
 				<< true_quaternion(2) << ", " << true_quaternion(3);
 	}
 
-	void validStateAfterOrientationInitialization()
-	{
-		quaternionVarianceBigEnoughAfterOrientationInitialization();
-		velocityAndPositionCloseToZero();
-		velocityAndPositionVarianceBigEnoughAfterOrientationInitialization();
-	}
-
-	void quaternionVarianceBigEnoughAfterOrientationInitialization()
+	void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f)
 	{
 		const matrix::Vector4f quat_variance = _ekf_wrapper.getQuaternionVariance();
-		const float quat_variance_limit = 0.0001f;
-		EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1)" << quat_variance(1);
-		EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2)" << quat_variance(2);
-		EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3)" << quat_variance(3);
+		EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1);
+		EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2);
+		EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3);
 	}
 
 	void velocityAndPositionCloseToZero()
@@ -101,20 +95,22 @@ public:
 				<< "vel = " << vel(0) << ", " << vel(1) << ", " << vel(2);
 	}
 
-	void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization()
+	void velocityVarianceBigEnoughAfterOrientationInitialization(float vel_variance_limit)
 	{
-		const Vector3f pos_var = _ekf->getPositionVariance();
 		const Vector3f vel_var = _ekf->getVelocityVariance();
 
-		const float pos_variance_limit = 0.01f; // Fake fusion obs var when at rest
-		EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(0)" << pos_var(0);
-		EXPECT_TRUE(pos_var(1) > pos_variance_limit) << "pos_var(1)" << pos_var(1);
-		EXPECT_TRUE(pos_var(2) > pos_variance_limit) << "pos_var(2)" << pos_var(2);
+		EXPECT_TRUE(vel_var(0) > vel_variance_limit) << "vel_var(0): " << vel_var(0);
+		EXPECT_TRUE(vel_var(1) > vel_variance_limit) << "vel_var(1): " << vel_var(1);
+		EXPECT_TRUE(vel_var(2) > vel_variance_limit) << "vel_var(2): " << vel_var(2);
+	}
 
-		const float vel_variance_limit = 0.0001f; // zero velocity update obs var when at rest
-		EXPECT_TRUE(vel_var(0) > vel_variance_limit) << "vel_var(0)" << vel_var(0);
-		EXPECT_TRUE(vel_var(1) > vel_variance_limit) << "vel_var(1)" << vel_var(1);
-		EXPECT_TRUE(vel_var(2) > vel_variance_limit) << "vel_var(2)" << vel_var(2);
+	void positionVarianceBigEnoughAfterOrientationInitialization(float pos_variance_limit)
+	{
+		const Vector3f pos_var = _ekf->getPositionVariance();
+
+		EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(0): " << pos_var(0);
+		EXPECT_TRUE(pos_var(1) > pos_variance_limit) << "pos_var(1): " << pos_var(1);
+		EXPECT_TRUE(pos_var(2) > pos_variance_limit) << "pos_var(2): " << pos_var(2);
 	}
 
 	void learningCorrectAccelBias()
@@ -144,8 +140,46 @@ TEST_F(EkfInitializationTest, initializeWithZeroTilt)
 	_sensor_simulator.simulateOrientation(quat_sim);
 	_sensor_simulator.runSeconds(_init_tilt_period);
 
+	EXPECT_TRUE(_ekf->control_status_flags().vehicle_at_rest);
+	EXPECT_FALSE(_ekf->control_status_flags().in_air);
+
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
+
 	initializedOrienationIsMatchingGroundTruth(quat_sim);
-	validStateAfterOrientationInitialization();
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	// Fake position fusion obs var when at rest sq(0.01f)
+	positionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
+
+	_sensor_simulator.runSeconds(1.f);
+	learningCorrectAccelBias();
+}
+
+TEST_F(EkfInitializationTest, initializeWithZeroTiltNotAtRest)
+{
+	const float pitch = math::radians(0.0f);
+	const float roll = math::radians(0.0f);
+	const Eulerf euler_angles_sim(roll, pitch, 0.0f);
+	const Quatf quat_sim(euler_angles_sim);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_sensor_simulator.simulateOrientation(quat_sim);
+	//_sensor_simulator.runSeconds(_init_tilt_period);
+	_sensor_simulator.runSeconds(7);
+
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
+
+	initializedOrienationIsMatchingGroundTruth(quat_sim);
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); // Fake position fusion obs var when at rest sq(0.5f)
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
 
 	_sensor_simulator.runSeconds(1.f);
 	learningCorrectAccelBias();
@@ -222,8 +256,20 @@ TEST_F(EkfInitializationTest, initializeHeadingWithZeroTilt)
 	_sensor_simulator.simulateOrientation(quat_sim);
 	_sensor_simulator.runSeconds(_init_tilt_period);
 
+	EXPECT_TRUE(_ekf->control_status_flags().vehicle_at_rest);
+	EXPECT_FALSE(_ekf->control_status_flags().in_air);
+
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
+	EXPECT_TRUE(_ekf->control_status_flags().yaw_align);
+
 	initializedOrienationIsMatchingGroundTruth(quat_sim);
-	validStateAfterOrientationInitialization();
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	// Fake position fusion obs var when at rest sq(0.01f)
+	positionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
 
 	_sensor_simulator.runSeconds(1.f);
 	learningCorrectAccelBias();
@@ -239,8 +285,44 @@ TEST_F(EkfInitializationTest, initializeWithTilt)
 	_sensor_simulator.simulateOrientation(quat_sim);
 	_sensor_simulator.runSeconds(_init_tilt_period);
 
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
+	EXPECT_TRUE(_ekf->control_status_flags().yaw_align);
+
 	initializedOrienationIsMatchingGroundTruth(quat_sim);
-	validStateAfterOrientationInitialization();
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	// Fake position fusion obs var when at rest sq(0.01f)
+	positionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
+
+	_sensor_simulator.runSeconds(1.f);
+	learningCorrectAccelBias();
+}
+
+TEST_F(EkfInitializationTest, initializeWithTiltNotAtRest)
+{
+	const float pitch = math::radians(30.0f);
+	const float roll = math::radians(60.0f);
+	const Eulerf euler_angles_sim(roll, pitch, 0.0f);
+	const Quatf quat_sim(euler_angles_sim);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_sensor_simulator.simulateOrientation(quat_sim);
+	//_sensor_simulator.runSeconds(_init_tilt_period);
+	_sensor_simulator.runSeconds(7);
+
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
+
+	initializedOrienationIsMatchingGroundTruth(quat_sim);
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	positionVarianceBigEnoughAfterOrientationInitialization(0.01f);
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
 
 	_sensor_simulator.runSeconds(1.f);
 	learningCorrectAccelBias();
@@ -253,13 +335,25 @@ TEST_F(EkfInitializationTest, initializeWithPitch90)
 	const Eulerf euler_angles_sim(roll, pitch, 0.0f);
 	const Quatf quat_sim(euler_angles_sim);
 
+	_ekf->set_in_air_status(false);
+	_ekf->set_vehicle_at_rest(true);
 	_sensor_simulator.simulateOrientation(quat_sim);
-	_sensor_simulator.runSeconds(_init_tilt_period);
+	//_sensor_simulator.runSeconds(_init_tilt_period);
+	_sensor_simulator.runSeconds(10);
+
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
 
-	initializedOrienationIsMatchingGroundTruth(quat_sim);
 	// TODO: Quaternion Variance is smaller and vel x is larger
 	// in this case than in the other cases
-	validStateAfterOrientationInitialization();
+
+	initializedOrienationIsMatchingGroundTruth(quat_sim);
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	// Fake position fusion obs var when at rest sq(0.01f)
+	positionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
 
 	_sensor_simulator.runSeconds(1.f);
 	learningCorrectAccelBias();
@@ -272,11 +366,22 @@ TEST_F(EkfInitializationTest, initializeWithRoll90)
 	const Eulerf euler_angles_sim(roll, pitch, 0.0f);
 	const Quatf quat_sim(euler_angles_sim);
 
+	_ekf->set_in_air_status(false);
+	_ekf->set_vehicle_at_rest(true);
 	_sensor_simulator.simulateOrientation(quat_sim);
-	_sensor_simulator.runSeconds(_init_tilt_period);
+	//_sensor_simulator.runSeconds(_init_tilt_period);
+	_sensor_simulator.runSeconds(10);
+
+	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);
 
 	initializedOrienationIsMatchingGroundTruth(quat_sim);
-	validStateAfterOrientationInitialization();
+	quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+
+	velocityAndPositionCloseToZero();
+
+	// Fake position fusion obs var when at rest sq(0.01f)
+	positionVarianceBigEnoughAfterOrientationInitialization(0.00001f);
+	velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f);
 
 	_sensor_simulator.runSeconds(1.f);
 	learningCorrectAccelBias();