EKF: Don't do magnetic field estimation without earth frame measurements

Use horizontal acceleration to check if yaw is observable independent of the magnetometer.
Use rotation about the vertical to check if mag raises are observable.
If neither yaw of mag biases are observable, save the magnetic field variances and switch to magnetic yaw fusion.
Use the last learned declination when using magnetic yaw fusion so that the yaw reference remains consistent.
When yaw or biases become observable, reinstate the saved variances and switch back to 3D mag fusion.

EKF/common.h

@@ -230,6 +230,8 @@ struct parameters {
 	float mag_innov_gate{3.0f};		// magnetometer fusion innovation consistency gate size (STD)
 	int mag_declination_source{7};		// bitmask used to control the handling of declination data
 	int mag_fusion_type{0};			// integer used to specify the type of magnetometer fusion used
+	float mag_acc_gate{0.5f};		// when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/s**2)
+	float mag_yaw_rate_gate{0.25f};		// yaw rate threshold used by mode select logic (rad/sec)
 
 	// airspeed fusion
 	float tas_innov_gate{5.0f};		// True Airspeed Innovation consistency gate size in standard deciation

EKF/control.cpp

@@ -888,9 +888,10 @@ void Ekf::controlMagFusion()
 	}
 
 	// If we are on ground, store the local position and time to use as a reference
+	// Also reset the flight alignment falg so that the mag fields will be re-initialised next time we achieve flight altitude
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = _state.pos(2);
-
+		_flt_mag_align_complete = false;
 	}
 
 	// checs for new magnetometer data tath has fallen beind the fusion time horizon
@@ -899,16 +900,66 @@ void Ekf::controlMagFusion()
 		// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
 		// or the more accurate 3-axis fusion
 		if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
-			// start 3D fusion if in-flight and height has increased sufficiently
-			// to be away from ground magnetic anomalies
-			// don't switch back to heading fusion until we are back on the ground
+			// Check if height has increased sufficiently to be away from ground magnetic anomalies
 			bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
-			bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved);
 
-			if (use_3D_fusion && _control_status.flags.tilt_align) {
-				// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
+			// Check if there has been enough change in horizontal velocity to make yaw observable
+			// Apply hysteresis to check to avoid rapid toggling
+			if (_yaw_angle_observable) {
+				_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
+			} else {
+				_yaw_angle_observable = _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
+			}
+			_yaw_angle_observable = _yaw_angle_observable && (_control_status.flags.gps || _control_status.flags.ev_pos);
+
+			// check if there is enough yaw rotation to make the mag bias states observable
+			if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
+				// initial yaw motion is detected
+				_mag_bias_observable = true;
+				_yaw_delta_ef = 0.0f;
+				_time_yaw_started = _imu_sample_delayed.time_us;
+			} else if (_mag_bias_observable) {
+				// monitor yaw rotation in 45 deg sections.
+				// a rotation of 45 deg is sufficient to make the mag bias observable
+				if (fabsf(_yaw_delta_ef) > 0.7854f) {
+					_time_yaw_started = _imu_sample_delayed.time_us;
+					_yaw_delta_ef = 0.0f;
+				}
+				// require sustained yaw motion of 50% the initial yaw rate threshold
+				float min_yaw_change_req =  0.5f * _params.mag_yaw_rate_gate * (1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started));
+				_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
+			} else {
+				_mag_bias_observable = false;
+			}
+
+			// record the last time that movement was suitable for use of 3-axis magnetometer fusion
+			if (_mag_bias_observable || _yaw_angle_observable) {
+				_time_last_movement = _imu_sample_delayed.time_us;
+			}
+
+			// decide whether 3-axis magnetomer fusion can be used
+			bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates
+					_control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies
+					(_flt_mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height
+					((_imu_sample_delayed.time_us - _time_last_movement) < 2000000); // Using 3-axis fusion for a minimum period after to allow for false negatives
+
+			// perform switch-over
+			if (use_3D_fusion) {
 				if (!_control_status.flags.mag_3D) {
-					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+					if (!_flt_mag_align_complete) {
+						// if transitioning into 3-axis fusion mode for the first time, we need to initialise the yaw angle and field states
+						_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+						_flt_mag_align_complete = true;
+					} else {
+						// reset the mag field covariances
+						zeroRows(P, 16, 21);
+						zeroCols(P, 16, 21);
+
+						// re-instate the last used variances
+						for (uint8_t index = 0; index <= 5; index ++) {
+							P[index+16][index+16] = _saved_mag_variance[index];
+						}
+					}
 				}
 
 				// use 3D mag fusion when airborne
@@ -916,9 +967,14 @@ void Ekf::controlMagFusion()
 				_control_status.flags.mag_3D = true;
 
 			} else {
-				// use heading fusion when on the ground
+				// save magnetic field state variances for next time
+				if (_control_status.flags.mag_3D) {
+					for (uint8_t index = 0; index <= 5; index ++) {
+						_saved_mag_variance[index] = P[index+16][index+16];
+					}
+					_control_status.flags.mag_3D = false;
+				}
 				_control_status.flags.mag_hdg = true;
-				_control_status.flags.mag_3D = false;
 			}
 
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
@@ -943,7 +999,7 @@ void Ekf::controlMagFusion()
 		}
 
 		// if we are using 3-axis magnetometer fusion, but without external 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 recommneded to prevent problem if the vehicle is static for extended periods of time
+		// fusing declination when gps aiding is available is optional, but recommended to prevent problem if the vehicle is static for extended periods of time
 		if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
 			_control_status.flags.mag_dec = true;
 

EKF/covariance.cpp

@@ -467,13 +467,6 @@ void Ekf::predictCovariance()
 
 	// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
 	if (_control_status.flags.mag_3D) {
-		// Check if we have just transitioned into 3-axis fusion and set the state variances
-		if (!_control_status_prev.flags.mag_3D) {
-			for (uint8_t index = 16; index <= 21; index++) {
-				P[index][index] = sq(fmaxf(_params.mag_noise, 0.001f));
-			}
-		}
-
 		// calculate variances and upper diagonal covariances for earth and body magnetic field states
 		nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10];
 		nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)/2;

EKF/ekf.cpp

@@ -379,8 +379,17 @@ void Ekf::predictState()
 	// update transformation matrix from body to world frame
 	_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
 
+	// Calculate an earth frame delta velocity
+	Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
+
+	// calculate a filtered horizontal acceleration with a 1 sec time constant
+	// this are used for manoeuvre detection elsewhere
+	float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
+	_accel_lpf_NE(0) = _accel_lpf_NE(0) * alpha + corrected_delta_vel_ef(0);
+	_accel_lpf_NE(1) = _accel_lpf_NE(1) * alpha + corrected_delta_vel_ef(1);
+
 	// calculate the increment in velocity using the current orientation
-	_state.vel += _R_to_earth * corrected_delta_vel;
+	_state.vel += corrected_delta_vel_ef;
 
 	// compensate for acceleration due to gravity
 	_state.vel(2) += _gravity_mss * _imu_sample_delayed.delta_vel_dt;
@@ -479,6 +488,15 @@ void Ekf::calculateOutputStates()
 	delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1) * dt_scale_correction;
 	delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction;
 
+	// calculate a yaw change about the earth frame vertical
+	float spin_del_ang_D = _R_to_earth_now(2,0) * delta_angle(0) +
+			_R_to_earth_now(2,1) * delta_angle(1) +
+			_R_to_earth_now(2,2) * delta_angle(2);
+	_yaw_delta_ef += spin_del_ang_D;
+
+	// calculate filtered yaw rate to be used by the magnetomer fusion type selection logic
+	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / _imu_sample_new.delta_ang_dt;
+
 	// correct delta velocity for bias offsets
 	Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias * dt_scale_correction;
 

EKF/ekf.h

@@ -266,6 +266,13 @@ private:
 
 	matrix::Dcm<float> _R_to_earth;	// transformation matrix from body frame to earth frame from last EKF predition
 
+	// used by magnetomer fusion mode selection
+	Vector2f _accel_lpf_NE;			// Low pass filtered horizontal earth frame acceleration (m/s**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 abut earth frame D axis (rad/sec)
+	bool _mag_bias_observable{false};	// true when there is enough rotation to make magnetomer bias errors observable
+	bool _yaw_angle_observable{false};	// true when there is enough horizontal acceleration to make yaw observable
+
 	float P[_k_num_states][_k_num_states] {};	// state covariance matrix
 
 	float _vel_pos_innov[6] {};	// innovations: 0-2 vel,  3-5 pos
@@ -328,7 +335,11 @@ private:
 	float _baro_hgt_offset{0.0f};		// baro height reading at the local NED origin (m)
 
 	// Variables used to control activation of post takeoff functionality
-	float _last_on_ground_posD{0.0f}; // last vertical position when the in_air status was false (m)
+	float _last_on_ground_posD{0.0f};	// last vertical position when the in_air status was false (m)
+	bool _flt_mag_align_complete{true};	// true when the in-flight mag field alignment has been completed
+	uint64_t _time_last_movement{0};	// last system time in usec that sufficient movement to use 3-axis magnetomer fusion was detected
+	float _saved_mag_variance[6] {};	// magnetic field state variances that have been saved for use at the next initialisation (Ga**2)
+	uint64_t _time_yaw_started{0};		// last system time in usec that a yaw rotation moaneouvre was detected
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
@@ -343,7 +354,7 @@ private:
 	float _terrain_var{1e4f};		// variance of terrain position estimate (m^2)
 	float _hagl_innov{0.0f};		// innovation of the last height above terrain measurement (m)
 	float _hagl_innov_var{0.0f};		// innovation variance for the last height above terrain measurement (m^2)
-	uint64_t _time_last_hagl_fuse;	// last system time in usec that the hagl measurement failed it's checks
+	uint64_t _time_last_hagl_fuse;		// last system time in usec that the hagl measurement failed it's checks
 	bool _terrain_initialised{false};	// true when the terrain estimator has been intialised
 	float _sin_tilt_rng{0.0f};		// sine of the range finder tilt rotation about the Y body axis
 	float _cos_tilt_rng{0.0f};		// cosine of the range finder tilt rotation about the Y body axis

EKF/ekf_helper.cpp

@@ -501,7 +501,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 void Ekf::calcMagDeclination()
 {
 	// set source of magnetic declination for internal use
-	if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
+	if (_flt_mag_align_complete) {
+		// Use value consistent with earth field state
+		_mag_declination = atan2f(_state.mag_I(1),_state.mag_I(0));
+	} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
 		// use parameter value until GPS is available, then use value returned by geo library
 		if (_NED_origin_initialised) {
 			_mag_declination = _mag_declination_gps;