Add kalman filter implementation

[nycrat]

Description

Adds implementation of Kalman filter from #3716 and some basic tests. Rest of the test cases should be added in #3727.

Testing Done

The basic unit tests

Resolved Issues

resolves #3723

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

[Andrewyx]

🚀 🚀 🚀