What if the update rate of our GPS sensor is low, e.g. 1 Hz? This means that the multi ordered queue gets blocked a lot, and interpolating far away from the GPS measurement is a bad idea. I thought it would be a good idea to insert the "missing measurement" some short time after the measurement has been received. Something like this:

GPS measurement is added at `t = 0.0s`

"No measurement" is added at `t + dt = 0.05s`

"No measurement" is added at `t + 2dt = 0.1s`

....
GPS measurement is added at `t = 1.0s`

"No measurement" is added at `t+dt = 1.05s`

.....

If I understood correctly, this means that we allow interpolating only between 0.0s and 0.05s, 1.0s and 1.05s and so on. First question is: does this make any sense, and second, if it does, what `dt`

should we use. My guess from above:

Based on your description, my guess is that dt should be something like 2 to 3 rangefinder periods - short enough not to block the multi ordered queue, and long enough to encompass 2 to 3 trajectory nodes which are stamped closest to the GPS measurement, i.e. interpolating the GPS data returns a result only for these nodes.