From afd6e54b2a8d273f11f56239e7654dc23760836a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Nov 2016 11:50:16 +1100 Subject: [PATCH] EKF: Allow for worst case timing jitter when setting observation buffer length --- EKF/estimator_interface.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 95318c0e36..c2c6c8ac11 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -353,8 +353,14 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter IMU_BUFFER_LENGTH = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; - // set the observaton buffer length to handle the minimum time of arrival between observations - OBS_BUFFER_LENGTH = (max_time_delay_ms / _params.sensor_interval_min_ms) + 1; + // set the observaton buffer length to handle the minimum time of arrival between observations in combination + // with the worst case delay from current time to ekf fusion time + // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter + uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceil((float)max_time_delay_ms * 0.5f)); + OBS_BUFFER_LENGTH = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1; + + // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) + OBS_BUFFER_LENGTH = math::min(OBS_BUFFER_LENGTH,IMU_BUFFER_LENGTH); ECL_INFO("EKF IMU buffer length = %i",(int)IMU_BUFFER_LENGTH); ECL_INFO("EKF observation buffer length = %i",(int)OBS_BUFFER_LENGTH);