[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>

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java

@@ -54,12 +54,12 @@ public class Drivetrain {
   numbers used  below are robot specific, and should be tuned. */
   private final DifferentialDrivePoseEstimator m_poseEstimator =
       new DifferentialDrivePoseEstimator(
+          m_kinematics,
           m_gyro.getRotation2d(),
           m_leftEncoder.getDistance(),
           m_rightEncoder.getDistance(),
           new Pose2d(),
-          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.01, 0.01),
-          VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(1)),
+          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
           VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
 
   // Gains are for example purposes only - must be determined for your own robot!
@@ -118,10 +118,7 @@ public class Drivetrain {
   /** Updates the field-relative position. */
   public void updateOdometry() {
     m_poseEstimator.update(
-        m_gyro.getRotation2d(),
-        new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()),
-        m_leftEncoder.getDistance(),
-        m_rightEncoder.getDistance());
+        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
 
     // 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 or timestamps.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java

@@ -56,12 +56,11 @@ public class Drivetrain {
   below are robot specific, and should be tuned. */
   private final MecanumDrivePoseEstimator m_poseEstimator =
       new MecanumDrivePoseEstimator(
+          m_kinematics,
           m_gyro.getRotation2d(),
           getCurrentDistances(),
           new Pose2d(),
-          m_kinematics,
-          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05),
-          VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01),
+          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
           VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
 
   // Gains are for example purposes only - must be determined for your own robot!
@@ -153,7 +152,7 @@ public class Drivetrain {
 
   /** Updates the field relative position of the robot. */
   public void 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 or timestamps.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java

@@ -4,7 +4,6 @@
 
 package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
 
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -12,9 +11,6 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.numbers.N5;
-import edu.wpi.first.math.numbers.N7;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Timer;
@@ -42,11 +38,9 @@ public class Drivetrain {
 
   /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
   below are robot specific, and should be tuned. */
-  private final SwerveDrivePoseEstimator<N7, N7, N5> m_poseEstimator =
-      new SwerveDrivePoseEstimator<N7, N7, N5>(
-          Nat.N7(),
-          Nat.N7(),
-          Nat.N5(),
+  private final SwerveDrivePoseEstimator m_poseEstimator =
+      new SwerveDrivePoseEstimator(
+          m_kinematics,
           m_gyro.getRotation2d(),
           new SwerveModulePosition[] {
             m_frontLeft.getPosition(),
@@ -55,9 +49,7 @@ public class Drivetrain {
             m_backRight.getPosition()
           },
           new Pose2d(),
-          m_kinematics,
-          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05),
-          VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01),
+          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
           VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
 
   public Drivetrain() {
@@ -89,12 +81,6 @@ public class Drivetrain {
   public void updateOdometry() {
     m_poseEstimator.update(
         m_gyro.getRotation2d(),
-        new SwerveModuleState[] {
-          m_frontLeft.getState(),
-          m_frontRight.getState(),
-          m_backLeft.getState(),
-          m_backRight.getState()
-        },
         new SwerveModulePosition[] {
           m_frontLeft.getPosition(),
           m_frontRight.getPosition(),