Robot Kalman Filters by sunghyuneun · Pull Request #3906 · UBC-Thunderbots/Software

sunghyuneun · 2026-08-27T21:13:16Z

Description

note: i am lazy and i want auto-formatting fixes so i marked it as open but it's not in a final state but it's 95% there i think (excluding physical tests)

Makes changes to robot filter. Notable changes:

Rewrote robot filter to utilize linear Kalman filter to get more accurate measurements of each robot's orientation and position
As Kalman filter is able to predict the robot position, if the robot temporarily goes missing in the data, it predicts the robot position at that time
Removes robot if it is missing for 10 updates, fixing RobotFilter does not remove expired robots if there are no new detections #1717.

Notably a lot of the files changed are part of PR #3900, because I built upon a lot of the work there. Files that should actually be reviewed include:
robot_filter.cpp, robot_filter.h, robot_filter_test.cpp, robot_team_filter.cpp, robot_team_filter_test.cpp

Testing with real robots is incomplete, and some constants should be changed.

Testing Done

Unit tests modified

Resolved Issues

Resolves #3701, fixes #1717.

Length Justification and Key Files to Review

Ball Kalman Filter is +529 -503, therefore this PR is +413, -101.

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

…into sunghyuneun/robot_filter

man fuck you

Andrewyx · 2026-10-01T05:20:04Z

    # Robot always not excessively dribbling
    always_validations = [
-        [BallAlwaysStaysInRegion([tbots_cpp.Circle(ball_location, 0.05)])]
+        [BallAlwaysStaysInRegion([tbots_cpp.Circle(ball_location, 0.5)])]


this is pretty significant relaxation, is it necessary?

Andrewyx · 2026-10-01T05:22:45Z

+// does not account for: deflections, uneven turf, and the tail of a kick. A kick
+// itself is far larger than this, but it is also abrupt enough that the outlier gates
+// catch it and reset the filter, so this does not need to cover one.
+constexpr double ACCELERATION_NOISE_M_PER_S_SQUARED = 5.0;


how were these values derived? Will they differ depending on camera? In that case, is anonymizing it here in this namespace the best location to put this?

Could these values be adjusted frequently? ie surface, camera, SSL vision quality? In that case, should we consider a dynamically adjustable set of datavalues

Andrewyx · 2026-10-01T05:26:18Z

+     * where it ended up. A ball moving at 5 m/s covers over 8 cm between frames at 60 Hz,
+     * so a test that only asked whether the ball was currently within its own radius of a
+     * surface would step straight over anything thin, and a goalpost is thin.
     *


given this desc, maybe a more apt naming scheme is appropriate

Andrewyx · 2026-10-01T05:27:13Z

+# Disable for kalman filter
+#cc_test(
+#    name = "ball_filter_test",
+#    srcs = ["ball_filter_test.cpp"],
+#    deps = [
+#        ":ball_filter",
+#        "//shared/test_util:tbots_gtest_main",
+#        "//software/world:field",
+#    ],
+#)


should this test be updated + enabled?

I started this PR by pulling the Ball Kalman Filter PR so there may be remnants remaining that I haven't removed yet.

Andrewyx · 2026-10-01T05:30:52Z


-void BallFilter::addNewDetectionsToBuffer(std::vector<BallDetection> new_ball_detections,
-                                          const Rectangle& filter_area)
+std::optional<Ball> BallFilter::estimateBallState(


how often is this called? is it a concern that we recalc a pseudo inverse on the fly every time?

…into sunghyuneun/robot_filter

sunghyuneun

andrew's comments moved to robot filter stuff where it's relevant.

sunghyuneun · 2026-10-03T16:02:00Z

+// catch it and reset the filter, so this does not need to cover one.
+// TODO: this does NOT apply to a robot but like whatever man lol
+constexpr double ACCELERATION_NOISE_M_PER_S_SQUARED       = 4.0;
+constexpr double ANG_ACCELERATION_NOISE_RAD_PER_S_SQUARED = 1.0;


Andrewyx comment -

how were these values derived? Will they differ depending on camera? In that case, is anonymizing it here in this namespace the best location to put this?

Could these values be adjusted frequently? ie surface, camera, SSL vision quality? In that case, should we consider a dynamically adjustable set of datavalues

sunghyuneun · 2026-10-03T16:03:43Z


-std::optional<Robot> RobotFilter::getFilteredData(
-    const std::vector<RobotDetection>& new_robot_data, const Timestamp& capture_timestamp,
+std::optional<Robot> RobotFilter::estimateRobotState(


how often is this called? is it a concern that we recalc a pseudo inverse on the fly every time?

…neun/tb-software into sunghyuneun/robot_filter

StarrryNight · 2026-10-11T00:06:11Z

if you have time can u merge master and resolve the conflicts

Andrewyx

Some comments. Ran out of time, so handing off to @StarrryNight / @nycrat if possible?

Andrewyx · 2026-10-11T01:25:22Z

 .vscode/
 .vscode/.*
 !.vscode/extensions.json
+*.code-workspace


intentional?

Andrewyx · 2026-10-11T01:26:12Z

 #include "software/networking/unix/threaded_proto_unix_sender.hpp"
 #include "software/simulation/er_force_simulator.h"

+// CSV file that the filtered robot state is logged to, alongside the ground truth


nit: "Ground truth state" is not informative. Unnecessary jargon and verbosity from AI here I assume. Really confusing what this supposed to actually be, assuming filtered vs real values from sim?

Andrewyx · 2026-10-11T01:32:35Z

+// Wraps an angle to [-pi, pi] so detected and actual orientations are directly
+// comparable (e.g. -3.1 rad and 3.1 rad aren't reported as a ~6.2 rad error)
+static double wrapAngle(double radians)
+{
+    return std::atan2(std::sin(radians), std::cos(radians));
+}


is this not some standard utility we have elsewhere? Seems like a common use case, or why are we "wrapping" in the first place?

Andrewyx · 2026-10-11T01:36:52Z

    Timestamp default_timestamp;
 };

+// Robot expires when it hasn't received its own data for at least 200 milliseconds.


why 200 ms?

arbitrary. we should change depending on real robot data.

…into sunghyuneun/robot_filter

StarrryNight added 30 commits April 3, 2026 19:02

temporary working evaluation

7ab30e9

add goalie_tactic_test evaluations

d2c3153

init

cf71504

finish header file and some cpp

1666a79

finish implmentation of kf

fc7076a

change ball filter

660c185

finish WITHOUT TUNING

1ade146

fix build issues

f5cfcd9

change import issues

15f5042

finish integrating kalman filter --> needs tuning

ca76a68

finish initial tuning

adaa05e

tuning values...

bb00252

merge

b4444f5

Tuning mostly done

6fbb0ed

add velocity gating but it doesnt work

7a6ef75

pre tigers

84fa388

tigers

379c81a

x

ce4307f

works

96855b2

update header

1826bf3

update cpp

b15b8d4

damping scales with time

6e24bf8

remove collision restitution logic

c2cf2aa

temporary remove ball filter tests

6d20e62

use first measurement direclty

5284bcb

Sensor fusion test ake ball move in future packet

f0b2ef8

only set velocity to 0 after 5 consecutive contact frames

6e3209b

Bound predictions and only widen covariance when bal visible

e56fda5

cleanup

502476d

force ball state with breambeam

be12302

sunghyuneun added 8 commits August 29, 2026 12:25

Merge branch 'ball_kalman_filter' into sunghyuneun/robot_filter

e2c914d

Merge branch 'master' of https://github.com/UBC-Thunderbots/Software …

7a3ecdc

…into sunghyuneun/robot_filter

finished robot_filter

ff93a53

Merge branch 'master' of https://github.com/UBC-Thunderbots/Software …

1603fe2

…into sunghyuneun/robot_filter

fix angle issues

673fbd1

robot filter tests pass

340baa2

Merge branch 'master' of https://github.com/UBC-Thunderbots/Software …

18d4569

…into sunghyuneun/robot_filter

Made robot team filter work

c7a51e7

sunghyuneun self-assigned this Sep 3, 2026

sunghyuneun marked this pull request as ready for review September 3, 2026 18:12

pre-commit-ci-lite Bot and others added 7 commits September 3, 2026 18:19

[pre-commit.ci lite] apply automatic fixes

c4c6b66

fix

e60d860

Merge branch 'master' of https://github.com/UBC-Thunderbots/Software …

6b2d185

…into sunghyuneun/robot_filter

i promise i'm not doing jackshit guys i promise

210d374

testing this trash bullshit

054c0af

man fuck you

changed back to time based

1d3d9f8

testing no verify

240a096

Andrewyx reviewed Oct 1, 2026

View reviewed changes

Merge branch 'master' of https://github.com/UBC-Thunderbots/Software …

200bfc6

…into sunghyuneun/robot_filter

sunghyuneun commented Oct 3, 2026

View reviewed changes

sunghyuneun and others added 6 commits October 3, 2026 09:11

Merging upstream changes

6e9d918

[pre-commit.ci lite] apply automatic fixes

7aea7b6

Fixed build errors

e7c7643

Merge branch 'sunghyuneun/robot_filter' of https://github.com/sunghyu…

74b2942

…neun/tb-software into sunghyuneun/robot_filter

idk man i'm loggin stuff and i'm trying to analyze on matlab idk man

ee3e1ea

[pre-commit.ci lite] apply automatic fixes

c4f160f

mkhlb self-requested a review October 7, 2026 17:08

Andrewyx reviewed Oct 11, 2026

View reviewed changes

Merge branch 'master' of https://github.com/UBC-Thunderbots/Software …

c770933

…into sunghyuneun/robot_filter

Conversation

sunghyuneun commented Aug 27, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Testing Done

Resolved Issues

Length Justification and Key Files to Review

Review Checklist

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

sunghyuneun left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

StarrryNight commented Oct 11, 2026

Uh oh!

Andrewyx left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

sunghyuneun commented Aug 27, 2026 •

edited

Loading