From 84d1435880ed21a89a30466694f251e5b2bbaca9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 31 Oct 2022 12:25:39 -0400 Subject: [PATCH] ekf2: ensure minimum output buffer sizing - buffer at least 2 samples for the IMU output predictor buffers - dropping below 2 becomes problematic for the minimum observation interval calculation and the vertical output buffer trapezoidal integration --- src/modules/ekf2/EKF/estimator_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 81f229c1af..bdecf7d1c2 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -503,7 +503,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f; // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter - _imu_buffer_length = ceilf(max_time_delay_ms / filter_update_period_ms); + _imu_buffer_length = math::max(2, (int)ceilf(max_time_delay_ms / filter_update_period_ms)); // set the observation 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