EKF3 "Rewind": Time Horizons

The Latency Reality

In a perfect world, sensors are instant. In the real world, especially with Android, latency is unavoidable.

1. Sensor Lag: The GPS chip takes time to calculate a fix.
2. OS Lag: Android buffers the location intent.
3. Network Lag: USB/UDP transmission takes milliseconds.
4. Processing Lag: The Flight Controller parses the MAVLink message.

By the time ArduPilot "sees" a position update, it is already 100-300ms old.

The Old Way: "Just Ignore It"

Legacy flight controllers (like APM 2.6) assumed GPS data was "now".

• Result: If the drone moved 1 meter in that 300ms delay, the controller would calculate an error based on where it is, not where it was. This lag in the feedback loop causes Pilot-Induced Oscillation (PIO) or "Toilet Bowling."

The EKF3 Way: Time Travel

The Extended Kalman Filter 3 (EKF3) maintains a History Buffer (or "Time Horizon").

1. The IMU Buffer

The EKF stores a ring buffer of IMU states (Gyro, Accel) for the last ~375ms (configurable via maxTimeDelay). It knows exactly where the drone was and how it was oriented for every millisecond of the recent past.

2. The Rewind Event

When a GPS_INPUT message arrives with a valid time_usec timestamp:

1. Recall: The EKF pauses the current frame.
2. Rewind: It looks up the vehicle state (Position/Velocity) from the buffer at the exact moment of time_usec.
3. Innovate: It compares the GPS measurement to the historical state to calculate the error (Innovation).
4. Propagate: It applies that correction to the historical state and then re-integrates the IMU data forward to the present moment.

3. The Result

The Flight Controller effectively "travels back in time" to apply the correction when the measurement actually happened. This mathematically eliminates the phase lag caused by Android/USB latency.

Critical Requirement: Accurate Timing

For this to work, the time_usec in the MAVLink message must be accurate to the millisecond relative to the flight controller's clock.

• Timesync: MAVLink GPS performs a TIMESYNC handshake on connection to calculate the offset between the phone's clock and the autopilot's boot time.
• Jitter: If the timestamp jitters, the "rewind" targets the wrong moment, introducing noise.

Source Code Reference

• Buffer Calculation: libraries/AP_NavEKF3/AP_NavEKF3_core.cpp - Logic for maxTimeDelay_ms.
• Recall Logic: libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp - See storedGPS.recall.