[wpimath] Use Odometry for internal state in Pose Estimation (#4668)

This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.

Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -56,8 +56,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
 }
 
 void Drivetrain::UpdateOdometry() {
-  m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState(),
-                         GetCurrentDistances());
+  m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentDistances());
 
   // Also apply vision measurements. We use 0.3 seconds in the past as an
   // example -- on a real robot, this must be calculated based either on latency

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h

@@ -77,11 +77,6 @@ class Drivetrain {
   // Gains are for example purposes only - must be determined for your own
   // robot!
   frc::MecanumDrivePoseEstimator m_poseEstimator{
-      m_gyro.GetRotation2d(),
-      GetCurrentDistances(),
-      frc::Pose2d{},
-      m_kinematics,
-      {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
-      {0.05, 0.05, 0.05, 0.05, 0.05},
-      {0.1, 0.1, 0.1}};
+      m_kinematics,  m_gyro.GetRotation2d(), GetCurrentDistances(),
+      frc::Pose2d{}, {0.1, 0.1, 0.1},        {0.1, 0.1, 0.1}};
 };