Improve localization

[openminddev]

A critical problem of this localization method is that when the initial pose is not correct, the odometer is still applied to the estimated pose, which makes it impossible to re-find the correct pose.

A potential solution for this problem is to

1. Implement multi-hypothesis tracking. Instead of tracking a single pose, maintain multiple pose hypotheses and let them compete.
2. Predict the pose only when the previous estimation is confident.
3. Add automatic re-localization on consecutive failures.
4. Add local grid search when confidence < threshold
5. Scale odometry by confidence