Ball Kalman Filter and KF base class

[StarrryNight]

Description

DO NOT REVIEW
(Changes for now also includes using true locations for validation. That has been moved to a separate PR #3699 .

• Implements a linear kalman filter for ball position approximation.
• Initializes a reusable kalman filter class that is currently being used in robot optimizer (Michael's KF on the Added Kalman Filter + Minor Motion Control Changes #3420 PR)

Testing Done

Currently:

• KF improves occluded predictions dramatically

• Master predicts position ~2cm better than kalman filter.

• KF is being tuned to predict position more accurately

• More heuristics are being added to the KF to account for the non-linearity caused by robot, like kicking, dribbling, etc.

• Test EKF/UKF and compare the results. We only need to change the prediction step in the kalman filter class to use a different algorithm, as the interface can be kept the same.

Results

• Collected data by running:
  ./tbots.py run thunderscope --enable_realism --record_stat x
• Collected around 2.6 million frames of data for linear regression filter (current) and kalman filter (this PR).
• • By running --record_stats 200
• Raw data were collected using the same pipeline, and post processed the same way.
• Following table records the key metrics, and direct comparison:

Overall Statistics

Category	Metric	Kalman Filter	Master	Δ
Total	Mean Err (m)	0.131	0.873	KF 6.7x better
Total	MSE	0.703	4.785	KF 6.8x better
Visible	Mean Err (m)	0.132	0.887	KF 6.7x better
Visible	MSE	0.711	4.872	KF 6.9x better
Occluded	Mean Err (m)	0.118	0.519	KF 4.4x better
Occluded	MSE	0.411	2.604	KF 6.3x better

Velocity-Stratified Error

Velocity Range	Metric	Kalman Filter	Master	Δ
[0,1] m/s	Mean Err (m)	0.133	0.923	KF 6.9x better
[0,1] m/s	MSE	0.741	5.082	KF 6.9x better
(1,2] m/s	Mean Err (m)	0.090	0.137	KF 1.5x better
(1,2] m/s	MSE	0.157	0.511	KF 3.3x better
(2,3] m/s	Mean Err (m)	0.128	0.095	Master 1.3x better
(2,3] m/s	MSE	0.120	0.074	Master 1.6x better
>3 m/s	Mean Err (m)	0.208	0.148	Master 1.4x better
>3 m/s	MSE	0.084	0.041	Master 2.0x better

Error Distribution

Error Range	Kalman Filter	Master	Δ
[0, 0.5] m	97.30%	79.88%	KF +17.42pp
(0.5, 1] m	0.27%	0.98%	KF 3.6x fewer
(1, 2] m	0.61%	2.45%	KF 4.0x fewer
(2, 3] m	0.43%	3.25%	KF 7.6x fewer
> 3 m	1.38%	13.44%	KF 9.7x fewer

Occlusion Handling

Master: Occlusion causes ball to stop completely at the last known position
https://github.com/user-attachments/assets/d5f64634-9ffa-4712-8221-cc4df7b6105f

Kalman filter: Model predicts ball motion under occlusion pretty accurately.
https://github.com/user-attachments/assets/826ca853-c4f1-446f-96fe-d684f13b0031

Resolved Issues

#3386
#3381
#3538

Length Justification and Key Files to Review

Review Checklist

It is the reviewers responsibility to also make sure every item here has been covered

• Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
• Remove all commented out code
• Remove extra print statements: for example, those just used for testing
• Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

[nycrat]

@StarrryNight just a note, this PR should wait for #3723 to be merged first. I think you already have the same kalman filter code, but I'll write tests first and merge into master