EKF: Improve in-flight mag error detection, recovery and isolation for fixed wing

EKF/common.h

@@ -413,6 +413,7 @@ union filter_control_status_u {
 		uint32_t fuse_beta   : 1; ///< 15 - true when synthetic sideslip measurements are being fused
 		uint32_t update_mag_states_only   : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer
 		uint32_t fixed_wing  : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
+		uint32_t mag_fault   : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used
 	} flags;
 	uint32_t value;
 };

EKF/control.cpp

@@ -443,8 +443,13 @@ void Ekf::controlGpsFusion()
 
 			if (do_reset) {
 				// Reset states to the last GPS measurement
-				resetPosition();
+				if (_control_status.flags.fixed_wing) {
-				resetVelocity();
+					// if flying a fixed wing aircraft, do a complete reset that includes yaw, velocity and position
+					realignYawGPS();
+				} else {
+					resetVelocity();
+					resetPosition();
+				}
 				ECL_WARN("EKF GPS fusion timeout - reset to GPS");
 
 				// Reset the timeout counters
@@ -1052,6 +1057,7 @@ void Ekf::controlMagFusion()
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = _state.pos(2);
 		_flt_mag_align_complete = false;
+		_num_bad_flight_yaw_events = 0;
 	}
 
 	// check for new magnetometer data that has fallen behind the fusion time horizon
@@ -1059,7 +1065,11 @@ 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) {
+		if (_control_status.flags.mag_fault) {
+			// do no magnetometer fusion at all
+			_control_status.flags.mag_hdg = false;
+			_control_status.flags.mag_3D = false;
+		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
 			// 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;
 

EKF/ekf.h

@@ -279,6 +279,7 @@ 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 moaneouvre was detected
+	uint8_t _num_bad_flight_yaw_events{0};	///< number of times a bad heading has been detected in flight and required a yaw reset
 
 	float P[_k_num_states][_k_num_states] {};	///< state covariance matrix
 

EKF/ekf_helper.cpp

@@ -341,31 +341,42 @@ void Ekf::alignOutputFilter()
 }
 
 // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
-// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
+// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
 bool Ekf::realignYawGPS()
 {
 	// Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15  for a reliable alignment
 	float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
 	if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
+		// check for excessive GPS velocity innovations
+		bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps;
+
+		// calculate GPS course over ground angle
+		float gpsCOG = atan2f(_gps_sample_delayed.vel(1),_gps_sample_delayed.vel(0));
+
 		// calculate course yaw angle
-		float velYaw = atan2f(_state.vel(1),_state.vel(0));
+		float ekfGOG = atan2f(_state.vel(1),_state.vel(0));
 
-		// calculate course yaw angle from GPS velocity
+		// Check the EKF and GPS course over ground for consistency
-		float gpsYaw = atan2f(_gps_sample_delayed.vel(1),_gps_sample_delayed.vel(0));
+		float courseYawError = gpsCOG - ekfGOG;
-
-		// Check the EKF and GPS course for consistency
-		float courseYawError = gpsYaw - velYaw;
-		courseYawError = wrap_pi(courseYawError);
 
 		// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
-		bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps;
 		bool badYawErr = fabsf(courseYawError) > 0.5f;
-		bool badMagYaw = (badYawErr && badVelInnov) || !_control_status.flags.yaw_align;
+		bool badMagYaw = (badYawErr && badVelInnov);
 
-		// correct yaw angle using GPS ground course if compass yaw bad
 		if (badMagYaw) {
+			_num_bad_flight_yaw_events ++;
+		}
+
+		// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
+		if (badMagYaw || !_control_status.flags.yaw_align) {
 			ECL_WARN("EKF bad yaw corrected using GPS course");
 
+			// declare the magnetomer as failed if a bad yaw has occurred more than once
+			if (_flt_mag_align_complete && (_num_bad_flight_yaw_events >= 2)) {
+				ECL_WARN("EKF stopping magnetometer use");
+				_control_status.flags.mag_fault = true;
+			}
+
 			// save a copy of the quaternion state for later use in calculating the amount of reset change
 			Quatf quat_before_reset = _state.quat_nominal;
 
@@ -376,51 +387,33 @@ bool Ekf::realignYawGPS()
 			// update transformation matrix from body to world frame using the current state estimate
 			_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
 
-			// Use the best conditioned of a 321 or 312 Euler sequence to avoid gimbal lock
+			// get quaternion from existing filter states and calculate roll, pitch and yaw angles
-			if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
+			Eulerf euler321(_state.quat_nominal);
-				// get quaternion from existing filter states and calculate roll, pitch and yaw angles
-				Eulerf euler321(_state.quat_nominal);
 
-				// calculate new filter quaternion states using previous Roll/Pitch and yaw angle corrected
+			// apply yaw correction
-				// for ground course discrepancy
+			if (!_flt_mag_align_complete) {
+				// This is our first flight aligment so we can assume that the recent change in velocity has occurred due to a
+				// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
 				euler321(2) += courseYawError;
-				_state.quat_nominal = Quatf(euler321);
+				_flt_mag_align_complete = true;
 
-				// update transformation matrix from body to world frame using the current state estimate
+			} else if (_control_status.flags.wind) {
-				_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
+				// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
+				// aligned with the wind relative GPS velocity vector
+				euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)) , (_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
 
 			} else {
-				// Calculate the 312 sequence euler angles that rotate from earth to body frame
+				// we don't have wind estimates, so align yaw to the GPS velocity vector
-				// See http://www.atacolorado.com/eulersequences.doc
+				euler321(2) = atan2f(_gps_sample_delayed.vel(1) , _gps_sample_delayed.vel(0));
-				Vector3f euler312;
-				euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));  // first rotation (yaw)
-				euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-				euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));  // third rotation (pitch)
 
-				// correct yaw angle for ground course discrepancy
-				euler312(0) += courseYawError;
-
-				// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
-				float c2 = cosf(euler312(2));
-				float s2 = sinf(euler312(2));
-				float s1 = sinf(euler312(1));
-				float c1 = cosf(euler312(1));
-				float s0 = sinf(euler312(0));
-				float c0 = cosf(euler312(0));
-
-				_R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
-				_R_to_earth(1, 1) = c0 * c1;
-				_R_to_earth(2, 2) = c2 * c1;
-				_R_to_earth(0, 1) = -c1 * s0;
-				_R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
-				_R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
-				_R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
-				_R_to_earth(2, 0) = -s2 * c1;
-				_R_to_earth(2, 1) = s1;
-
-				_state.quat_nominal = Quatf(_R_to_earth);
 			}
 
+			// calculate new filter quaternion states using corected yaw angle
+			_state.quat_nominal = Quatf(euler321);
+
+			// update transformation matrix from body to world frame using the current state estimate
+			_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
+
 			// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
 			resetVelocity();
 			resetPosition();
@@ -465,6 +458,7 @@ bool Ekf::realignYawGPS()
 			_state_reset_status.quat_counter++;
 
 			// the alignment using GPS has been successful
+			_control_status.flags.yaw_align = true;
 			return true;
 
 		} else {