Skip to content

AP_NavEKF3: size IMU buffer from the actual EKF time step - #35211

Open
LachlanConn wants to merge 1 commit into
ArduPilot:masterfrom
LachlanConn:pr/ekf3-imu-buffer-step
Open

LachlanConn wants to merge 1 commit into
ArduPilot:masterfrom
LachlanConn:pr/ekf3-imu-buffer-step

Conversation

@LachlanConn

Copy link
Copy Markdown
Contributor

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)

  • Checked by a human programmer
  • Non-functional change
  • No-binary change
  • Infrastructure change (e.g. unit tests, helper scripts)
  • Automated test(s) verify changes (e.g. unit test, autotest)
  • Tested manually, description below (e.g. SITL)
  • Tested on hardware
  • Logs attached
  • Logs available on request
Loop rate Master slots Master horizon (core 0) Branch slots Branch horizon (core 0)
50 Hz 17 319–321 ms 17 319–321 ms
100 Hz 17 159–161 ms 21 199–201 ms
150 Hz 17 213–215 ms 17 213–215 ms
200 Hz 17 159–165 ms 21 199–205 ms
250 Hz 17 191–193 ms 17 191–193 ms
400 Hz 17 198–201 ms 17 198–201 ms

SITL Plane with GPS1_DELAY_MS=200. A temporary debug print reported imu_buffer_length and 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.

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.
@rmackay9 rmackay9 added the EKF label Nov 16, 2026
@rmackay9

Copy link
Copy Markdown
Contributor

Hi @LachlanConn,

Txs for this. During testing did you find that samples were being lost because the IMU buffer was too small?

@LachlanConn

Copy link
Copy Markdown
Contributor Author

@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.

@AP-Review

Copy link
Copy Markdown

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head 33f37837d5.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35211/1.html#pr35211

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

  • libraries/AP_NavEKF3/AP_NavEKF3_core.cpp:96: the length still truncates, and the FIFO only spans len-1 steps. With a 200 ms GPS delay the horizon is 192 ms at 250/500/1000 Hz and 194 ms at 350 Hz. readGpsData() also subtracts localFilterTimeStep_ms/2 (6 ms), so an on-time fix is still clamped to imuDataDelayed even at 100/200/400 Hz, which is the late-GPS clamping you describe. Rounding the quotient up is not enough on its own (250 Hz would reach 204 ms). It needs a deliberate margin for the half-step correction and jitter, keeping in mind that a longer buffer also delays the fusion horizon for every sensor.
Minor (1)
  • libraries/AP_NavEKF3/AP_NavEKF3_core.cpp:42: the new dtEkfAvg value only lasts through bootstrap, because InitialiseVariables() resets it to EKF_TARGET_DT before the filter starts. If a better starting value is wanted, set it there too. Otherwise this assignment has no effect on the running filter.

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.

@github-actions

Copy link
Copy Markdown

Global Size Summary (Total Flash delta in bytes)

Built from 33f3783 rebased onto a995235

Board Periph tracker blimp bootloader copter heli iofw plane rover sub
DrotekP3Pro _ _ _ * _ _ _ _ _ _
Durandal _ +12 +12 _ +12 +12 _ +12 +12 +12
Hitec-Airspeed * _ _ _ _ _ _ _ _ _
KakuteF7 _ +36 +36 _ +36 +36 _ +36 +32 +36
MambaH743v4 _ +12 +12 _ +12 +12 _ +12 +12 +12
MatekF405-CAN _ _ _ * _ _ _ _ _ _
MatekF405 _ +36 +36 _ +36 +36 _ +36 +32 +36
MatekH743-bdshot _ +12 +12 _ +12 +12 _ +12 +12 +12
Pixhawk1-1M _ +36 +36 _ +36 +36 _ +36 +32 +36
disco _ +16 +26 _ +4 +10 _ +22 +14 +26
f103-GPS * _ _ _ _ _ _ _ _ _

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants