From d8831c5133e15ebd1fe209ac8cbd67d0247c0571 Mon Sep 17 00:00:00 2001 From: dominiknatter <46818571+dominiknatter@users.noreply.github.com> Date: Mon, 29 Jun 2020 18:00:29 +0200 Subject: [PATCH] ekf2: Ignore optical flow samples with too large integration time spans. Fixes #14165 Within ekf2, optical flow messages (amongst others) are fused to the state estimates. It might occur that optical flow sensors report unreliable and unrealistic spikes. In that case, the state estimator went crazy so far and just ignored optical flow values from that moment on. The common thread for all these spikes seems to be a too high integration time span. Therefore, this fix adds a simple logic that ignores unrealistically high integration time spans. As a threshold, 1 second was chosen. Reported-by: Dominik Natter --- src/modules/ekf2/ekf2_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index bab3656fad..974caa9f82 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1061,7 +1061,8 @@ void Ekf2::Run() flow.time_us = optical_flow.timestamp; if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && - PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) { + PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && + flow.dt < 1) { _ekf.setOpticalFlowData(flow); }