From 9183c479a52dba8a0948eaca0a1225c49896497b Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 26 Aug 2024 16:37:17 +0200 Subject: [PATCH] ekf2: correctly compute vel variance from flow variance Co-authored-by: Marco Hauswirth --- .../EKF/aid_sources/optical_flow/optical_flow_control.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 3299577a28..95db812fae 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -217,7 +217,9 @@ void Ekf::resetFlowFusion() { ECL_INFO("reset velocity to flow"); _information_events.flags.reset_vel_to_flow = true; - resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed)); + + const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(_flow_sample_delayed); + resetHorizontalVelocityTo(_flow_vel_ne, flow_vel_var); // reset position, estimate is relative to initial position in this mode, so we start with zero error if (!_control_status.flags.in_air) {