Added Kalman Filter + Minor Motion Control Changes

[mkhlb]

Please fill out the following before requesting review on this PR

Description

Many changes were done.

Implemented a kalman filter for thunderloop that composes IMU data to track robot position more accurately.

Now when following trajectories, additionally to trajectories current target velocity, add a proportional component based on error from predicted position.

Testing Done

Moved robot.

Tested IMU variance, got about 0.0019 deg variance, expected about 0.0016 deg.

Need to do much further testing with new robots.

Resolved Issues

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

[github-actions[bot]]

This PR is stale because it has been open for 30 days with no activity. Remove stale label or comment or this will be closed in 5 days.

[github-actions[bot]]

This PR is stale because it has been open for 30 days with no activity. Remove stale label or comment or this will be closed in 5 days.

[github-actions[bot]]

This PR was closed because it has been stalled for 5 days with no activity.