[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/native/include/frc/kinematics/DifferentialDriveKinematics.h

@@ -6,6 +6,7 @@
 
 #include <wpi/SymbolExports.h>
 
+#include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
 #include "units/angle.h"
@@ -64,6 +65,20 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
             chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
   }
 
+  /**
+   * Returns a twist from left and right distance deltas using
+   * forward kinematics.
+   *
+   * @param leftDistance The distance measured by the left encoder.
+   * @param rightDistance The distance measured by the right encoder.
+   * @return The resulting Twist2d.
+   */
+  constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
+                              const units::meter_t rightDistance) const {
+    return {(leftDistance + rightDistance) / 2, 0_m,
+            (rightDistance - leftDistance) / trackWidth * 1_rad};
+  }
+
   units::meter_t trackWidth;
 };
 }  // namespace frc