diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp index a0b814988a..c1aa518047 100644 --- a/src/modules/ekf2/EKF/wind.cpp +++ b/src/modules/ekf2/EKF/wind.cpp @@ -60,6 +60,11 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, _information_events.flags.reset_wind_to_ext_obs = true; resetWindTo(wind, wind_var); + + // reset the horizontal velocity variances to allow the velocity states to be pulled towards + // a solution that is aligned with the newly set wind estimates + static constexpr float hor_vel_var = 25.0f; + P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var); } void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 123b5e37a4..24b92a1ad4 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -536,8 +536,10 @@ void EKF2::Run() } if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) { - _ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2, - vehicle_command.param4); + if (_ekf.control_status_flags().wind_dead_reckoning) { + _ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2, + vehicle_command.param4); + } } } }