ekf2: yaw estimator additional validity checks

Author: dagar

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -393,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const
 		return false;
 	}
 
-	return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
+	const float yaw_var = _yawEstimator.getYawVar();
+
+	return (yaw_var > 0.f)
+	       && (yaw_var < sq(_params.EKFGSF_yaw_err_max))
+	       && PX4_ISFINITE(yaw_var);
 }
 
 bool Ekf::isYawFailure() const

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp

@@ -167,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy,
 			const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
 			_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta);
 		}
+
+		if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) {
+			reset();
+		}
 	}
 }