From 868bc010878adaf5a124fc0c9d6cef4045fa06ac Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 5 May 2018 16:33:01 +1000 Subject: [PATCH] EKF: Relax minimum required range finder measurement rate --- EKF/terrain_estimator.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index c36721d156..221088ff77 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -207,16 +207,17 @@ void Ekf::get_hagl_innov_var(float *hagl_innov_var) // check that the range finder data is continuous void Ekf::checkRangeDataContinuity() { - // update range data continuous flag (2Hz ie 500 ms) + // update range data continuous flag (1Hz ie 1000 ms) /* Timing in micro seconds */ - /* Apply a 1.0 sec low pass filter to the time delta from the last range finder updates */ - _dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - _dt_update) + _dt_update * + /* 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); - _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 1e6f); + _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 2e6f); - if (_dt_last_range_update_filt_us < 5e5f) { + if (_dt_last_range_update_filt_us < 1e6f) { _range_data_continuous = true; } else {