fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 14:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Paul Riseborough
2016-11-07 11:50:16 +11:00
parent 403424d55d
commit afd6e54b2a
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+8 -2
View File
@@ -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);