Delayed fusion in robot_localization

[7DoF]

Hi, I'm currently trying to use the delayed fusion code inside the ros_filter.cpp.

Setup details:
SITL with IMU data and gps data.

nodes running:

1. navset_transform node to convert gps to gps_odom
2. ukf node: to use this gps_odom and imu to produce /odometry/filtered

my objective is to generate odometry out of gps data and imu data, at the same time verify if the delayed fusion code is correctly working or not.

custom setup: when a gps topic is published, i store it in a queue and hold it for 2 seconds and then publish the same gps topic. such that this gps data is "OLD" w.r.t current system time. furthermore, all the gps data is delayed by 2 seconds using this approach [ unlike the standard case where no measurement is delayed but only few measurements are only older ]

I an not able to fully understand how the delayed fusion is happening in this case.

from what i understand:

1. first the data is received by the integrateMeasurement() function which checks if the measurement data is OLD, by comparing filter's latest state timestamp with measurement's timestamp ( based on the top element of measurementQueue.top())
2. then the function RevertTo() is called and the timestamp of oldest delayed measurement is passed inside it.
3. ones inside the revertTo() function, the first while loop checks which state of the filter corresponds to the timestamp passed on to the revertTo() function (oldest measurement stamp ) and the filter's current state is updated to the state whose timestamp corresponds to this old measurement.
4. in the next while loop, data from the measurementHistory queue is pushed to MeasurementQueue queue. and popped from measurementHistory queue.
5. next the control returns back to IntegrateMeasurement() function.
6. there is a while loop which checks if the MeasurementQueue is not empty and then the top measurement of this queue is taken and processMeasurement() function is called on this measurement.
7. the definition of this function is written inside filter_base.cpp . first its checked if the filter is initialized, if its initialized we calculate delta which is diff of current measurement timestamp VS filter's latest state's timestamp.
8. if delta is non zero that means first predict() is called then correct() is called.
9. if delta < 0 [ in case there is no gps measurement and just imu is there ] then just correct() is called.

my understanding of how delayed fusion can be handled is :

1. we check if there is a delayed measurement [ handled]
2. if it exists, we revert the filter to timestamp of that delayed measurement. and meanwhile, all the measurements after that delayed measurement will also be stored in a queue. [handled]
3. we start from that reverted state and integrate all the measurements in this queue until the queue is empty. and then we predict the current state after all these calculations. [not sure if its handled here]

I'm not sure if the delayed fusion handling is completely done inside robot_localization. Can someone help me in understanding if there is anything I'm missing.

appreciate any help. thanks.