Time adjustment of data samples in obs buffer

I’m working on the estimation system and found that a time adjustement is performed to the sensor’s data samples before pushing them to their respective buffers using the formula:

adjusted_timestamp = sensor_sample_time - EKF2_(sensor)_DELAY - ekf2_update_interval/2

What’s the reason behind performing this time adjustment and why this specific formula? if you can add a scenario to better explain the importance of this time adjustment I will be grateful since I already tried to understand the reason behind it but I find I’m still lacking in how the ekf2 module handle the delays

Gentlemen any suggestions? @JulianOes, @Paul_Riseborough, @bresch

Hi,

  • sensor_sample_time is the timestamp of when the sensor was sampled in the driver (the measurement was done by the sensor a couple of ms before that)
  • EKF2_(sensor)_DELAY is the expected delay between the actual measurement of the sensor and timestamping done by the driver

So sensor_sample_time - EKF2_(sensor)_DELAY is the actual timestamp of the measurement.

Additionally, ekf2_update_interval/2 is just a “trick” to get the sample out of the ring buffer as close as possible to the delayed-horizon:
If the measurement is just before the midpoint interval between 2 EKF updates, fuse it during the update before the actual timestamp and if it occurs after the midpoint, fuse it during the update after the actual timestamp. That way, the maximum time alignment error is <= ekf2_update_interval while otherwise the samples would always be fused in the update after the actual timestamp and the time alignment error would be between 0 and ekf2_update_interval.

I hope this is clear enough.

1 Like