Pre-EKF Robot Localizer Clean up

[StarrryNight]

Description

This PR is the base of the "redesign robot localizer" PR stack. It fixes the following issues:

1. Add getter functions in primitive executer for previous target velocity, and use that as the control input of the robot localizer
2. Instead of snapping incoming vision data to the closest step before, we use the actual time of the vision data. This corrects the subsaquent process model and covariances as they depends on elapsed time, dt.
3. Instead of storing multiple 6x6 matrices in each filter step, we compute the matrices dynamically on rewind. This operation is not expensive as it only involves a multiplications for each matrix.
4. Instead of using optionals in FilterStep struct and checking for them to determine update/prediction, we store an enum that tells us what type of step this was.
5. Generate measurement matrix and process matrices with helper functions to improve code quality
6. Rename several functions for clarity

Testing Done

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