Kalman Filter Tests

[adrianchan787]

Description

Kalman Filter mathematical and behavioural unit tests.

Split the tests up into 1-D, 2-D, 3-D.

• [ X ] 1-D i.e. just position, these I test mathematically (the equations are simple because transposing does nothing). Also test behaviourally (similar to the current unit tests, checking expected behaviour for intuitive test cases).
• [ X ] 2-D and 3-D i.e. position and velocity, tested just behaviourally as the equations are very complex.

Rough partitioning for behaviour:

• 1-D, 2-D, 3-D
• same and different dimensions
• Process noise very high, measurement noise very high, equal noise
• No process model, no control model, no measurement model
• Filters out inaccurate sensors
• Uses very accurate sensors

(please lmk if there's any more tests worth adding)

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

• [ X ] 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.
• [ X ] Remove all commented out code
• [ X ] Remove extra print statements: for example, those just used for testing
• [ X ] Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

If needed I can comment on the derivation of the mathematical equations.

[nycrat]

Great work! I know some of the tests have lots of boilerplate, especially with the Eigen library. I think it's fine to leave it like this cause it would be quite difficult to re-write and somehow communicate the meanings of each value supplied into these matrices. The comments are good for explanation