Kalman Filter Tests

[adrianchan787]

Description

Draft PR of Kalman Filter Tests.

Was planning to split the tests up into 1-D, 2-D, 3-D.

• 1-D i.e. just position, for these I was thinking because the equations are not hard (since transposes don't matter as 1-D), I just compare it mathematically, using regular algebra. This seems to work (tests pass rn). Not sure if separately testing predict and update is a good idea (it's easy to implement, but to my understanding Kalman always uses predict and update together sequentially?)
• 2-D i.e. position and velocity, for this I don't think using the mathematical equation is a good idea so probably something similar to the current unit tests where test boundaries (i.e. very large measurement covariance). Not sure which process models I should test or need to test with though.
• 3-D i.e. position, velocity, and angular velocity. Similar to 2-D.

I have no clue what specifically I should do for 2-D or 3-D though lol.

Testing Done

Tests pass so far.

Resolved Issues

resolves #3727

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