ekf2: robustify wind init using airspeed

Tests showed that sideslip fusion often starts just before airspeed
fusion, resetting the wind states to 0 instead of using the airspeed
data. A quick look at the wind uncertainty allows to know if the current
wind estimate is meaningful or if we should rather reset it using the
airspeed data.

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -114,7 +114,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			ECL_INFO("starting airspeed fusion");
 
 			// If starting wind state estimation, reset the wind states and covariances before fusing any data
-			if (!_control_status.flags.wind) {
+			// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
+			const Vector2f wind_var_xy = getWindVelocityVariance();
+
+			if (!_control_status.flags.wind || (wind_var_xy(0) + wind_var_xy(1) > sq(_params.initial_wind_uncertainty))) {
 				// activate the wind states
 				_control_status.flags.wind = true;
 				// reset the wind speed states and corresponding covariances

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -85,6 +85,21 @@ bool EkfWrapper::isIntendingExternalVisionHeightFusion() const
 	return _ekf->control_status_flags().ev_hgt;
 }
 
+void EkfWrapper::enableBetaFusion()
+{
+	_ekf_params->beta_fusion_enabled = true;
+}
+
+void EkfWrapper::disableBetaFusion()
+{
+	_ekf_params->beta_fusion_enabled = false;
+}
+
+bool EkfWrapper::isIntendingBetaFusion() const
+{
+	return _ekf->control_status_flags().fuse_beta;
+}
+
 void EkfWrapper::enableGpsFusion()
 {
 	_ekf_params->gnss_ctrl |= GnssCtrl::HPOS | GnssCtrl::VEL;

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -69,6 +69,10 @@ public:
 	/* void disableExternalVisionHeightFusion(); */
 	bool isIntendingExternalVisionHeightFusion() const;
 
+	void enableBetaFusion();
+	void disableBetaFusion();
+	bool isIntendingBetaFusion() const;
+
 	void enableGpsFusion();
 	void disableGpsFusion();
 	bool isIntendingGpsFusion() const;

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -131,3 +131,38 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
 	EXPECT_NEAR(height_after_pressure_correction, expected_height_after_pressure_correction, 1e-2f);
 
 }
+
+TEST_F(EkfAirspeedTest, testResetWindUsingAirspeed)
+{
+	const Vector3f simulated_velocity_earth(-3.6f, 8.f, 0.0f);
+	const Vector2f airspeed_body(15.f, 0.0f);
+	_ekf_wrapper.enableGpsFusion();
+	_sensor_simulator._gps.setVelocity(simulated_velocity_earth);
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(11);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_is_fixed_wing(true);
+
+	// Simulate the fact that the sideslip can start immediately, without
+	// waiting for a measurement sample.
+	_ekf_wrapper.enableBetaFusion();
+	_sensor_simulator.runSeconds(0.1);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
+
+	_sensor_simulator.startAirspeedSensor();
+	_sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0));
+	_sensor_simulator.runSeconds(0.1);
+
+	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
+
+	const Vector2f vel_wind_earth = _ekf->getWindVelocity();
+
+	const Dcmf R_to_earth_sim(_quat_sim);
+	const Vector3f vel_wind_expected = simulated_velocity_earth - R_to_earth_sim * (Vector3f(airspeed_body(0),
+					   airspeed_body(1), 0.0f));
+
+	EXPECT_NEAR(vel_wind_earth(0), vel_wind_expected(0), 1.f);
+	EXPECT_NEAR(vel_wind_earth(1), vel_wind_expected(1), 1.f);
+}