Repository navigation
AP_NavEKF3: size IMU buffer from the actual EKF time step - #35211
LachlanConn wants to merge 1 commit into
Conversation
readIMUData() runs a prediction once the accumulated IMU time is within half an IMU sample of EKF_TARGET_DT, so the EKF step is the nearest whole number of IMU samples to 12 ms. At 100 Hz and 200 Hz loop rates this is 10 ms, but the IMU buffer length was calculated assuming 12 ms per element, so the fusion time horizon was only 5/6 of the largest sensor delay (e.g. 160 ms instead of 200 ms for the default GPS lag). Every sample from a sensor with the largest delay was then clamped to the oldest buffer time and fused late. Calculate the expected step from the loop rate using the same rounding and use it for the buffer length when it is shorter than EKF_TARGET_DT. The buffer length is unchanged for loop rates where the step is 12 ms or more, which includes the default Copter rate of 400 Hz.
|
Hi @LachlanConn, Txs for this. During testing did you find that samples were being lost because the IMU buffer was too small? |
|
@rmackay9 No samples are lost from what I saw. The main thing I saw is that with GPS_DELAY_MS set to default 200ms all GPS timestamps are getting clamped to the end of the window. It means we don't really have any tolerance for a late GPS message due to something like CAN bus delay because it gets over clamped and causes a big spike in innovations and causes a lane switch. |
|
Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head This change does make the IMU buffer match the real EKF step at 100/200 Hz without shortening it at any other rate, but it still does not fully cover the GPS delay. Should fix
Minor (1)
The one red CI job, sitltest-plane-tests1c, passed locally apart from LoggedNamedValueString, which fails the same way on master with an older pymavlink. Its remote cause was not confirmed. |
Global Size Summary (Total Flash delta in bytes)Built from 33f3783 rebased onto a995235
|
Summary
Sizing the EKF buffer window based on actual EKF time step not assumed 12ms constant
Classification & Testing (check all that apply and add your own)
SITL Plane with
GPS1_DELAY_MS=200. A temporary debug print reportedimu_buffer_lengthand the measured fusion horizon (imuDataNew.time_ms - imuDataDelayed.time_ms, min/max over 10 s). Core 1 horizons ran longer at some rates because the inter-EKF scheduler defers its predictions when CPU time is low. That only lengthens steps, so the shortest step is still the worst case the buffer is sized for.Description
We are sizing our EKF buffer window based on largest delay from sensors but there is an assumption that each EKF time step is 12ms. While this is basically true with 400Hz at 200Hz the EKF time step is actually 10ms (2 IMU samples).
We size the buffer on number of 12ms steps would be in our maxTimeDelay.
For a 200ms time delay that's 16 (+1 for jitter) but when we times 16 but the actual 10ms EKF time step at 200Hz looprate we get a window sizing of 160ms not the 200ms we were aiming for.