Run RobotLocalizer on Fullsystem to fuse vision detections with motor + IMU data

[williamckha]

Description of the task

Fullsystem currently estimates robot state in RobotFilter using only vision detections. Meanwhile, each robot already runs a RobotLocalizer with a Kalman filter that fuses vision data, motor hall sensor velocities, and IMU data into a more accurate estimate. This means the AI makes decisions + plans trajectories using robot velocity estimates that are worse than what each robot already knows about itself.

We should look into running the same RobotLocalizer on fullsystem so that both it and the robots share one consistent estimator.

Acceptance criteria

• Have RobotFilter use RobotLocalizer for robot state estimation instead of the simple moving average it's currently using
• Send motor velocities + IMU data back to fullsystem via RobotStatus so it can be incorporated in RobotFilter estimate
• Generalize RobotLocalizer so that we rollback state + replay updates not just for delayed vision data, but also for delayed hall sensor / IMU data (since RobotStatus will be delayed)
• RobotFilter needs to be able to handle robots not on the field (e.g. if robot disappears from vision for a while, it should not be detected) and robots being put back on the field (need to reset RobotLocalizer state)

Blocked By