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.