[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

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h

@@ -60,8 +60,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
     m_previousAngle = pose.Rotation();
     m_gyroOffset = m_pose.Rotation() - gyroAngle;
 
-    m_prevLeftDistance = 0_m;
-    m_prevRightDistance = 0_m;
+    m_prevLeftDistance = leftDistance;
+    m_prevRightDistance = rightDistance;
   }
 
   /**

wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h

@@ -32,5 +32,22 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
    * Distance driven by the rear-right wheel.
    */
   units::meter_t rearRight = 0_m;
+
+  /**
+   * Checks equality between this MecanumDriveWheelPositions and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const MecanumDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this MecanumDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const MecanumDriveWheelPositions& other) const = default;
 };
 }  // namespace frc

wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h

@@ -66,11 +66,9 @@ class SwerveDriveOdometry {
 
   /**
    * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
+   * integration of the pose over time. This also takes in an angle parameter
+   * which is used instead of the angular rate that is calculated from forward
+   * kinematics.
    *
    * @param gyroAngle The angle reported by the gyroscope.
    * @param modulePositions The current position of all swerve modules. Please
@@ -90,7 +88,8 @@ class SwerveDriveOdometry {
   Rotation2d m_previousAngle;
   Rotation2d m_gyroOffset;
 
-  wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions;
+  wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
+      wpi::empty_array};
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc

@@ -13,11 +13,15 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
     SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
     const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
     const Pose2d& initialPose)
-    : m_kinematics(kinematics),
-      m_pose(initialPose),
-      m_previousModulePositions(modulePositions) {
+    : m_kinematics(kinematics), m_pose(initialPose) {
   m_previousAngle = m_pose.Rotation();
   m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+  for (size_t i = 0; i < NumModules; i++) {
+    m_previousModulePositions[i] = {modulePositions[i].distance,
+                                    modulePositions[i].angle};
+  }
+
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
 }
@@ -30,7 +34,10 @@ void SwerveDriveOdometry<NumModules>::ResetPosition(
   m_pose = pose;
   m_previousAngle = pose.Rotation();
   m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  m_previousModulePositions = modulePositions;
+
+  for (size_t i = 0; i < NumModules; i++) {
+    m_previousModulePositions[i].distance = modulePositions[i].distance;
+  }
 }
 
 template <size_t NumModules>
@@ -39,11 +46,13 @@ const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
     const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
   auto moduleDeltas =
       wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
-  for (size_t index = 0; index < modulePositions.size(); index++) {
+  for (size_t index = 0; index < NumModules; index++) {
     auto lastPosition = m_previousModulePositions[index];
     auto currentPosition = modulePositions[index];
     moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
                            currentPosition.angle};
+
+    m_previousModulePositions[index].distance = modulePositions[index].distance;
   }
 
   auto angle = gyroAngle + m_gyroOffset;
@@ -55,7 +64,6 @@ const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
 
   m_previousAngle = angle;
   m_pose = {newPose.Translation(), angle};
-  m_previousModulePositions = modulePositions;
 
   return m_pose;
 }