From 3b32ee41660afd4785c374355e0fdefdae83e9b9 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Sep 2019 14:01:31 +0200 Subject: [PATCH] Flow aiding - Reset state when flow is enabled only if it is the only position/velocity aiding sensor. Until now, it was alway resetting if the vehicle does not have gps or external vision. This caused a reset/glitch at every stop (when range data gets valid) --- EKF/control.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 12e4f74329..225901fdbc 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -465,20 +465,19 @@ void Ekf::controlOpticalFlowFusion() _control_status.flags.opt_flow = true; _time_last_of_fuse = _time_last_imu; - // if we are not using GPS then the velocity and position states and covariances need to be set - if (!_control_status.flags.gps || !_control_status.flags.ev_pos) { + // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set + const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos); + if (flow_aid_only) { resetVelocity(); resetPosition(); // align the output observer to the EKF states alignOutputFilter(); - } } } else if (!(_params.fusion_mode & MASK_USE_OF)) { _control_status.flags.opt_flow = false; - } // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period