Account for measurement delay in localization

[Flova]

Summary

Until now we rated the current state of the localization particles always with a delayed measurement. The particles are moved using the odometry "instantly" and are therefore not where the localization expects them to be at a different place compared to when the measurement was taken. Therefore, particles that lag behind were preferred, as they better matched the observations, which were assumed to be also instantly.

The solution is to store the observation timestamp and move all particles temporary back by the amount we have walked since the measurement, as determined by the odometry before rating them. This solution works also for multiple asynchronous measurement sources.

I hope that this enables more aggressive path planning parameters with less oscillations, as the localization state is not delayed as much.

I also did some cleanup, like simplifying the motion model math significantly and removing the field boundary input (we never used it) as well as other unused features I encountered.

Here is a plot of the robots center of mass and the position estimated by the localization. It needs to be noted that the robot walks very fast and the vision had an artificial pipeline delay of 1 second to test the effectiveness of the approach:

Checklist

• Run colcon build
• Write documentation
• Test on your machine
• Test on the robot
• Create issues for future work
• Triage this PR and label it

[Flova]

While testing I noticed that this smoothing is not really needed anymore btw.
Sry for being unrelated here.