EKF: Allow for worst case timing jitter when setting observation buffer length

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);