Robot Kalman Filters

[sunghyuneun]

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