diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 33985779c20c..7f8539b1cb0e 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -93,7 +93,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr; const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f); const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant - && (is_airspeed_consistent || !_control_status.flags.wind); // if wind isn't already estimated, the states are reset when starting airspeed fusion + && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning); if (_control_status.flags.fuse_aspd) { if (continuing_conditions_passing) { @@ -118,17 +118,16 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) } else if (starting_conditions_passing) { ECL_INFO("starting airspeed fusion"); - // If starting wind state estimation, reset the wind states and covariances before fusing any data - // 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.inertial_dead_reckoning && !is_airspeed_consistent) { + resetVelUsingAirspeed(airspeed_sample); - 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 + } else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) { + // If starting wind state estimation, reset the wind states and covariances before fusing any data + // Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet. resetWindUsingAirspeed(airspeed_sample); } + _control_status.flags.wind = true; _control_status.flags.fuse_aspd = true; } @@ -247,3 +246,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample) _aid_src_airspeed.time_last_fuse = _time_delayed_us; } + +void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample) +{ + const float euler_yaw = getEulerYaw(_R_to_earth); + + // Estimate velocity using zero sideslip assumption and airspeed measurement + Vector2f horizontal_velocity; + horizontal_velocity(0) = _state.wind_vel(0) + airspeed_sample.true_airspeed * cosf(euler_yaw); + horizontal_velocity(1) = _state.wind_vel(1) + airspeed_sample.true_airspeed * sinf(euler_yaw); + + float vel_var = NAN; // Do not reset the velocity variance as wind variance estimate is most likely not correct + resetHorizontalVelocityTo(horizontal_velocity, vel_var); + + _aid_src_airspeed.time_last_fuse = _time_delayed_us; +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e98062623664..2a8c402887ed 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -788,6 +788,7 @@ class Ekf final : public EstimatorInterface // Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip void resetWindUsingAirspeed(const airspeedSample &airspeed_sample); + void resetVelUsingAirspeed(const airspeedSample &airspeed_sample); #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 482f7671eaed..513e6d057b3d 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -100,6 +100,11 @@ bool EkfWrapper::isIntendingBetaFusion() const return _ekf->control_status_flags().fuse_beta; } +bool EkfWrapper::isIntendingAirspeedFusion() const +{ + return _ekf->control_status_flags().fuse_aspd; +} + void EkfWrapper::enableGpsFusion() { _ekf_params->gnss_ctrl |= static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL); diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index beb43e64e3be..43753e9bc548 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -73,6 +73,8 @@ class EkfWrapper void disableBetaFusion(); bool isIntendingBetaFusion() const; + bool isIntendingAirspeedFusion() const; + void enableGpsFusion(); void disableGpsFusion(); bool isIntendingGpsFusion() const; diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 28964035c96c..d284b959e3be 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -166,3 +166,39 @@ TEST_F(EkfAirspeedTest, testResetWindUsingAirspeed) EXPECT_NEAR(vel_wind_earth(0), vel_wind_expected(0), 1.f); EXPECT_NEAR(vel_wind_earth(1), vel_wind_expected(1), 1.f); } + +TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) +{ + const Vector3f simulated_velocity_earth(-3.6f, 8.f, 0.0f); + const Vector2f airspeed_body(15.f, 0.0f); + _sensor_simulator.runSeconds(10); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _ekf->set_is_fixed_wing(true); + + const double latitude_new = -15.0000005; + const double longitude_new = -115.0000005; + const float altitude_new = 1500.0; + + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new); + _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, 50.f, 0); + + // Simulate the fact that the sideslip can start immediately, without + // waiting for a measurement sample. + _ekf_wrapper.enableBetaFusion(); + _sensor_simulator.runSeconds(1.f); + EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion()); + + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0)); + _sensor_simulator.runSeconds(1.f); + EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); + + EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); + const Vector3f vel = _ekf->getVelocity(); + EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-3f); + const Vector2f vel_wind_earth = _ekf->getWindVelocity(); + EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); + EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f); +}