When we integrated 1323's pose management algo, we moved it into Drivetrain creating many issues with resetting the pose and creating a cluttered Drive class. This PR moves it back into RobotStateEstimator and uses the InterpolatingTreeMaps. Both "tried and true" and alternate are there and the algorithms are the same. However, this code does NEED testing before merging.
When we integrated 1323's pose management algo, we moved it into Drivetrain creating many issues with resetting the pose and creating a cluttered Drive class. This PR moves it back into RobotStateEstimator and uses the InterpolatingTreeMaps. Both "tried and true" and alternate are there and the algorithms are the same. However, this code does NEED testing before merging.