Ball Kalman Filter

[StarrryNight]

Description

TLDR:
Implements a kalman filter with several heuristics to estimate the position of the ball. The kalman filter nominally functions as well as the old filter, while removing catastrophic failures (position error >3) by 30x and reducing overall positional MSE by 29x. In addition, velocity estimation is also improved by approximately 3x.

This PR replaces the current linear-regression based ball tracker. The original ball filter test doesn't apply anymore and they are temporarily disabled.

Results

Results taken by running thunderscope in ci mode, record stats, enable realism.

• Collected like 100k frames for kf and 1m for master (Ill test more later)
• Computed the absolute position error between simulation position and sensor fusion result
• Same above for velocity
• MATLAB analysis

MSE & Median Error Comparison

Category / Slice	Metric	Master	Kalman Filter	Comparison (Master / KF)
Overall: Total	Median Error	0.00581	0.00512	1.13× lower
	MSE	4.78470	0.16213	29.51× lower
Overall: Visible	Median Error	0.00530	0.00449	1.18× lower
	MSE	4.87160	0.15365	31.71× lower
Overall: Occluded	Median Error	0.01531	0.01407	1.09× lower
	MSE	2.60390	0.30343	8.58× lower
Velocity: [0, 1]	MSE	5.08160	0.16520	30.76× lower
Velocity: (1, 2]	MSE	0.51110	0.16720	3.06× lower
Velocity: (2, 3]	MSE	0.07440	0.10240	0.73× (Master 1.38× lower)
Velocity: > 3	MSE	0.04140	0.02230	1.86× lower

---

Error Distribution Comparison

Error Range	Master (%)	Kalman Filter (%)	Comparison
[0, 0.5] (Nominal)	79.88%	98.87%	+18.99% frames in low-error band
(0.5, 1]	0.98%	0.22%	4.45× fewer errors
(1, 2]	2.45%	0.32%	7.66× fewer errors
(2, 3]	3.25%	0.14%	23.21× fewer errors
> 3 (Catastrophic)	13.44%	0.44%	30.55× fewer extreme outliers

Non-technical explanation

A kalman filter is an algorithm that combines the prediction of a physical model and measurement from the real world. Every frame, based on how confident we are of the prediction and measurement, we mathematically blend them into one state estimate.
E.g. (not mathematically accurate)
Prediction (confidence 70%): position at 1
Measurement (confidence 30%): Position at 2
We take the state estimate at 1.3

The prediction and measurement is a gaussian probability. The variance of each individual variable is recorded in the covariance matrix. It is the analog of the uncertainty we have on each one of them. The higher the covariance, the lower our confidence. Now, you can see how we blend two gaussian probabilities into one final estimation:

If we run a vanilla kalman filter on a ball moving and slowing down, our estimate will almost always be in the middle of the prediction and the measurement. Our physical model will always be ahead (it doesnt take account of friction) and our measurement will always be behind (it has a delay). This is precisely what the kalman filter does when the ball is in free flight, when there are no robots, field boundaries, or goals nearby.

Heuristic number 1: Collision handling

When the ball is near an obstacle (robot, goalpost, or boundary), our standard ballistic model breaks down because external impulses (kicks/bounces) cannot be anticipated from vision alone. In these zones:

• We increase prediction covariance, forcing the filter to prioritize incoming vision measurements over model extrapolation.

• We damp/zero the velocity state to prevent momentum-carryover artifacts (velocity spikes/overshoot) when contact occurs.

Heuristic number 2: Bad packet handling

We also need to know when to reject bad positional data as they are noisy. There are two gating applied. First, the mahalanobis gating is used as a statistical method. It basically reject position data based on how many standard deviations it is from the measurement gaussian distribution. Second, is a physical gating. We calculate how fast the ball must travel to reach the new packet destination from the current state. If it is too high, the packet is probably wrong and we reject it

Note that we count how many rejects we have had in a row. If we have 3 bad packets in a row that means our current estimate is probably wrong. Then we will use the new packet and reset our filter.

Heuristic number 3: Occlusion handling

When the ball is blocked by a robot, vision measurements cease entirely. In this state, Heuristic 1 is bypassed so the filter relies strictly on the physical motion model to propagate the ball's position until we get packets again.

Heuristic number 4: Breakbeam

After our breambeam triggers and we put the ball in dribble position, we force update the kalman filter state to be the dribble position with 0 velocity. In addition, the thresholding used for breakbeam trigger is also updated to use the kalman filter prediction of the frame instead of the initial occlusion frame.

Resolved Issues

#2169
#2752
#3689
#3009

Length Justification and Key Files to Review

Tests

• Sim test
• Test normal tracking
• Test occlusion tracking
• Test breakbeam on an actual robot (dont break an actual breakbeam)
• Add unit tests with the breakbeam detections
• update num_dropped_detections_before_ball_not_in_dribbler

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

[sunghyuneun]

This pr was very informative, thank you.

Crazy work on your description.

[sunghyuneun]

typo (veloity)

[sunghyuneun]

unsure how true this statement is, but perhaps it might be unclear to readers that we're using a linear kalman filter to track, which is why we have a control size?

might be beneficial to mention that in one of the comments above.

[sunghyuneun]

The param filter_area was deleted and replaced with field, and in ball_filter.cpp it was replaced with a field.fieldBoundary(), so the comment above and this parameter should change to reflect that

[sunghyuneun]

appreciate how this is much clearer