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

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java

@@ -6,6 +6,7 @@ package edu.wpi.first.math.kinematics;
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Twist2d;
 
 /**
  * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
@@ -57,4 +58,20 @@ public class DifferentialDriveKinematics {
         chassisSpeeds.vxMetersPerSecond
             + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
   }
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the given left and right side
+   * distance deltas. This method is often used for odometry -- determining the robot's position on
+   * the field using changes in the distance driven by each wheel on the robot.
+   *
+   * @param leftDistanceMeters The distance measured by the left side encoder.
+   * @param rightDistanceMeters The distance measured by the right side encoder.
+   * @return The resulting Twist2d.
+   */
+  public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
+    return new Twist2d(
+        (leftDistanceMeters + rightDistanceMeters) / 2,
+        0,
+        (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java

@@ -134,6 +134,7 @@ public class SwerveDriveOdometry {
 
       moduleDeltas[index] =
           new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
+      previous.distanceMeters = current.distanceMeters;
     }
 
     var angle = gyroAngle.plus(m_gyroOffset);
@@ -145,11 +146,7 @@ public class SwerveDriveOdometry {
 
     m_previousAngle = angle;
     m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    for (int index = 0; index < m_numModules; index++) {
-      m_previousModulePositions[index] =
-          new SwerveModulePosition(
-              modulePositions[index].distanceMeters, modulePositions[index].angle);
-    }
+
     return m_poseMeters;
   }
 }