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

[williamckha]

LGTM, just two nits and a comment

[williamckha]

Suggested change

	// So rollback ever step
	// So rollback every step

[williamckha]

I don't think moving the IMU service poll here is a good idea (frankly it makes things inconsistent since now we're collecting IMU data at a different time than the motor data).

This raises a problem with the order we're doing things right now. Currently this is what we're doing:

1. Run the predict step for robot localizer, which gives us the priori estimate for time x
2. Run the update step for robot localizer using the sensor data for time x-1 we collected in the previous loop iteration. This is incorrect. We should be using the current sensor data (time x) for this update step to get the posteriori estimate for time x
3. Use the INCORRECT posteriori estimate to step the primitive executor
4. Collect sensor data for the current time x

What we should be doing is:

1. Collect sensor data for the current time x
2. Run the predict step for robot localizer, which gives us the priori estimate for time x
3. Run the update step for robot localizer USING the sensor data we just collected, which gives the posteriori estimate for time x
4. Use the posteriori estimate to step the primitive executor

What makes this difficult right now is that we're both collecting sensor data + commanding motors in the same MotorService::poll. We'd need to have separate methods so that we get sensor data before running robot localizer, and then command the motors after stepping primitive executor.

Food for thought

[StarrryNight]

I agree with that. I will open a ticket about current robot localizer issues including:

• RTT time being fixed to 15ms
• Lack of real robot testing :(
• seperating motor poll and motor update
  On the note at this we might as well rename the poll methods to smth more indicative. For now, I will revert the imu polling to after update robot localizer, so the time is at least consistent.

[williamckha]

I'm not a fan of TODOs in-code tbh because we never clean them up lol. If you must put a TODO, attach an issue number so we can check (in the future) if it's complete or not and get rid of the TODO

[StarrryNight]

I'll just remove this. The process model and varaicnes seems to be fine and we can debug this if robot testing ends up bad

[the-anh-hoang]

LGTM!