Add support for message lag on measurements

[chadrockey]

When for instance GPS measurements arrive, it's possibly they have significant lag. Applying this update to the current position results in poor results.

It's better to keep the state of the filter around for a bit and then update the filter at time t based on the innovation from time t_-n.

[chadrockey]

Difference between no latency correction:

and latency correction (by altering bag file recorded timestamps):

I was able to run at faster update rates without too much trouble as well.