[FEATURE] FOM Pose Estimator

[AT727]

Description

Uses 1690 Orbit's pose calculations to create a trust-based / FOM pose estimator. As of right now:

• standard deviation / FOM calculations are incorrect and need to be tuned and made.
• Collision detection has not worked in simulation since I couldn't get the accelerometer to output any data.
• Skid detection math does work; expected values are between 1 - 4

DrivetrainReal uses the estimator for setStateStdDevs() and logs the collision detection.

Type of change

Please delete options that are not relevant.

• New feature (non-breaking change which adds functionality)
• In progress (fix or feature that isn't completed / ignore checklist if this is marked)

Checklist:

• My code follows the style guidelines of this project
• Someone have performed a self-review of my code
• I have commented my code, particularly in hard-to-understand areas
• My changes generate no new warnings
• I have added tests that prove my fix is effective or that my feature works
• New and existing unit tests pass locally with my changes
• Any dependent changes have been merged and published in downstream modules