Robot Localizer and Kalman Filter Improvements

[StarrryNight]

Description of the task

See RFC:
https://docs.google.com/document/d/1LegJFjdp46UwrqtJCju_HqZqy0jOxJ7LbhzV1TsXBN8/edit?tab=t.0#heading=h.lm38suuf4qg

Acceptance criteria

This issue is planned to be splitted into 3 stacked PRs of:

• Implement EKF class
• Replace LKF with EKF in robot localizer
• Implement other changes listed in the RFC

With subsequent opened issues as:

• Computationally optimize linear algebra math used in the EKF (or LKF too)
• Refactor the way we set up kalman filter constants (like initial covariance, process matrix, etc)
• Implement class hierachy with LKF and EKF, perhaps making an interface of kalman filters, and implementations of LKF and EKF. (just class redesign, this allows us to try out UKF as well)
• Improve encapsulation of KF class

Blocked By