From a2260e53da48495644e128562ad80c1c276e7bef Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 18 Jan 2022 16:55:45 +0100
Subject: [PATCH] ekf2: replace yaw failure detection criteria

The existing logic using the angle between velocity vectors failed to
determine a yaw failure in practice because the state velocity is often
too small compared to its uncertainty to be used. In all the failures
reported, the yaw emergency estimator converged properly and yaw reset
would have fixed the issue.
A much simpler check using the yaw difference between the main EKF
and the emergency estimator is now used to tell if the vel/pos update
failure is most likely caused by a wrong heading.
---
 src/modules/ekf2/EKF/control.cpp     | 26 --------------------------
 src/modules/ekf2/EKF/ekf.h           |  2 +-
 src/modules/ekf2/EKF/gps_control.cpp | 17 +++++++++++++++--
 3 files changed, 16 insertions(+), 29 deletions(-)

diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index e3fe101ac5..e1bc47a721 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -1150,32 +1150,6 @@ void Ekf::controlAuxVelFusion()
 	}
 }
 
-
-bool Ekf::isVelStateAlignedWithObs() const
-{
-	/* Do sanity check to see if the innovation failures is likely caused by a yaw angle error
-	 * by measuring the angle between the velocity estimate and the last velocity observation
-	 * Only use those vectors if their norm if they are larger than 4 times their noise standard deviation
-	 */
-	const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared();
-	const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared();
-
-	const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f));
-	const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f));
-
-	if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) {
-		const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());
-		const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq);
-
-		if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) {
-			// The angle between the observation and the velocity estimate is greater than 25 degrees
-			return false;
-		}
-	}
-
-	return true;
-}
-
 bool Ekf::hasHorizontalAidingTimedOut() const
 {
 	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index a9269bdcb1..60ffdffd69 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -822,7 +822,7 @@ private:
 	void controlGpsFusion();
 	bool shouldResetGpsFusion() const;
 	bool hasHorizontalAidingTimedOut() const;
-	bool isVelStateAlignedWithObs() const;
+	bool isYawFailure() const;
 	void processYawEstimatorResetRequest();
 	void processVelPosResetRequest();
 
diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp
index 4ad248a1b5..eade218a4e 100644
--- a/src/modules/ekf2/EKF/gps_control.cpp
+++ b/src/modules/ekf2/EKF/gps_control.cpp
@@ -71,14 +71,13 @@ void Ekf::controlGpsFusion()
 					fuseGpsVelPos();
 
 					if (shouldResetGpsFusion()){
-						const bool is_yaw_failure = !isVelStateAlignedWithObs();
 						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
 
 						/* A reset is not performed when getting GPS back after a significant period of no data
 						 * because the timeout could have been caused by bad GPS.
 						 * The total number of resets allowed per boot cycle is limited.
 						 */
-						if (is_yaw_failure
+						if (isYawFailure()
 						    && _control_status.flags.in_air
 						    && !was_gps_signal_lost
 						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
@@ -188,6 +187,20 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
 }
 
+bool Ekf::isYawFailure() const
+{
+	if (!isYawEmergencyEstimateAvailable()) {
+		return false;
+	}
+
+	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
+				? getEuler321Yaw(_R_to_earth)
+				: getEuler312Yaw(_R_to_earth);
+	const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
+
+	return fabsf(yaw_error) > math::radians(25.f);
+}
+
 void Ekf::processYawEstimatorResetRequest()
 {
 	/* The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.