Handle time delayed measurement data without filter time delay #263
Description
One simple way to deal with delayed measurement data is outlined in #262. But often a delayed output of the filtered pose is not acceptable, and a more sophisitcated solution should be able to deal with the time delays without operating the whole filter at a time delay.
There are currently two methods from literature that I am aware of. [1] relies on hardware triggers to indicate when a new measurement will be made. It copies the current state whenever a trigger is received and then uses that to process the measurement when it has been computed (which can be much later). It actually uses stochastic cloning to apply the relative pose measruements as discussed in #261. In general we cannot assume the availibility of such triggers, so this method seems not applicable.
In contrast, what is done in [2] is keeping a fixed-length window of past states and measurements. Whenever a delayed measurement arrives, the closest state in the window is picked and the prediction and correction from the saved measurements is repeated. This seems like something that could be done in r_l, at the cost of complexity for the bookkeeping, as well as the computational cost for the repeated prediction / correction steps.
In the following I extracted two figures from the papers referenced in [2], which illustrate this quite will.
For a single time-delayed sensor:
The same extended to multiple sensors:
[1] http://www6.in.tum.de/Main/Publications/Burschka2012a.pdf
[2] https://github.com/ethz-asl/ethzasl_msf