From a7245229cbc3916f66be9d948875ff019dc41d5f Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Mon, 3 Sep 2018 00:26:43 -0400 Subject: [PATCH] EKF : Fix divergence when optical flow is not fused for a long time (#503) * terrain_estimator : guard against case where latest range sample is newer than IMU sample * EKF : control : correct detection of no optical flow fusion over a time period --- EKF/control.cpp | 2 +- EKF/terrain_estimator.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index e0baca1956..9b37f151d6 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -420,7 +420,7 @@ void Ekf::controlOpticalFlowFusion() _control_status.flags.opt_flow = false; _time_last_of_fuse = 0; - } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) { + } else if (_time_last_imu - _time_last_of_fuse > (uint64_t)_params.no_gps_timeout_max) { _control_status.flags.opt_flow = false; } diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 16cb9a29ee..0891492526 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -206,7 +206,7 @@ void Ekf::checkRangeDataContinuity() /* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */ float alpha = 0.5f * _dt_update; _dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha * - (_time_last_imu - _time_last_range); + (_imu_sample_delayed.time_us - _range_sample_delayed.time_us); _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);