From 3b0f522951223719f88f54e18d0eff0303ce8943 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 1 Jul 2024 07:34:22 +0200 Subject: [PATCH] correctly compute variances Signed-off-by: RomanBapst --- src/modules/ekf2/EKF/wind.cpp | 5 ++++- src/modules/ekf2/EKF2.cpp | 4 ++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp index 5bdde187cb..a0b814988a 100644 --- a/src/modules/ekf2/EKF/wind.cpp +++ b/src/modules/ekf2/EKF/wind.cpp @@ -46,10 +46,13 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, // wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F); + const float wind_direction_var = sq(math::radians(wind_direction_accuracy)); + const float wind_speed_var = sq(wind_speed_accuracy); + matrix::SquareMatrix P_wind; Vector2f wind; - sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_accuracy, wind_direction_accuracy, &wind, &P_wind); + sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction_rad, wind_speed_var, wind_direction_var, &wind, &P_wind); const Vector2f wind_var = P_wind.diag(); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 17d55ab740..526eee5140 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -534,6 +534,10 @@ void EKF2::Run() accuracy, timestamp_observation); } } + + 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); + } } }