[wpimath] Position Delta Odometry for Swerve (#4493)

wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java

@@ -4,16 +4,16 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 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.N1;
 import edu.wpi.first.math.numbers.N3;
@@ -26,10 +26,16 @@ import java.util.function.BiConsumer;
  * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
  * drop-in for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
  *
+ * <p>The generic arguments to this class define the size of the state, input and output vectors
+ * used in the underlying {@link UnscentedKalmanFilter Unscented Kalman Filter}. {@link Num States}
+ * must be equal to the module count + 3. {@link Num Inputs} must be equal to the module count + 3.
+ * {@link Num Outputs} must be equal to the module count + 1.
+ *
  * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If your loops are
  * faster or slower than the default of 20 ms, then you should change the nominal delta time using
- * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d,
- * Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Nat, Nat,
+ * Nat, Rotation2d, Pose2d, SwerveModulePosition[], SwerveDriveKinematics, Matrix, Matrix, Matrix,
+ * double)}.
  *
  * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
  * want; if you never call it, then this class will behave mostly like regular encoder odometry.
@@ -37,21 +43,25 @@ import java.util.function.BiConsumer;
  * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
  * (y):
  *
- * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
- * position, and heading.
+ * <p><strong> x = [x, y, theta, s_0, ... s_n]ᵀ </strong> in the field coordinate system containing
+ * x position, y position, and heading, followed by the distance travelled by each wheel.
  *
- * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
+ * <p><strong> u = [v_x, v_y, omega, v_0, ... v_n]ᵀ </strong> containing x velocity, y velocity, and
+ * angular rate in the field coordinate system, followed by the velocity measured at each wheel.
  *
  * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
  * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
  */
-public class SwerveDrivePoseEstimator {
-  private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
+public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Outputs extends Num> {
+  private final UnscentedKalmanFilter<States, Inputs, Outputs> m_observer;
   private final SwerveDriveKinematics m_kinematics;
-  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
+  private final BiConsumer<Matrix<Inputs, N1>, Matrix<N3, N1>> m_visionCorrect;
   private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
 
+  private final Nat<States> m_states;
+  private final Nat<Inputs> m_inputs;
+  private final Nat<Outputs> m_outputs;
+
   private final double m_nominalDt; // Seconds
   private double m_prevTimeSeconds = -1.0;
 
@@ -63,29 +73,41 @@ public class SwerveDrivePoseEstimator {
   /**
    * Constructs a SwerveDrivePoseEstimator.
    *
+   * @param states The size of the state vector.
+   * @param inputs The size of the input vector.
+   * @param outputs The size of the outputs vector.
    * @param gyroAngle The current gyro angle.
    * @param initialPoseMeters The starting pose estimate.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
    * @param kinematics A correctly-configured kinematics object for your drivetrain.
    * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-   *     meters and radians.
-   * @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number
-   *     to trust sensor readings from the gyro less. This matrix is in the form [theta], with units
-   *     in radians.
+   *     model's state estimates less. This matrix is in the form [x, y, theta, s_0, ... s_n]ᵀ, with
+   *     units in meters and radians, then meters.
+   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
+   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
+   *     is in the form [theta, s_0, ... s_n], with units in radians followed by meters.
    * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
    *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
    *     theta]ᵀ, with units in meters and radians.
    */
   public SwerveDrivePoseEstimator(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Nat<Outputs> outputs,
       Rotation2d gyroAngle,
       Pose2d initialPoseMeters,
+      SwerveModulePosition[] modulePositions,
       SwerveDriveKinematics kinematics,
-      Matrix<N3, N1> stateStdDevs,
-      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> localMeasurementStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {
     this(
+        states,
+        inputs,
+        outputs,
         gyroAngle,
         initialPoseMeters,
+        modulePositions,
         kinematics,
         stateStdDevs,
         localMeasurementStdDevs,
@@ -96,36 +118,72 @@ public class SwerveDrivePoseEstimator {
   /**
    * Constructs a SwerveDrivePoseEstimator.
    *
+   * @param states The size of the state vector.
+   * @param inputs The size of the input vector.
+   * @param outputs The size of the outputs vector.
    * @param gyroAngle The current gyro angle.
    * @param initialPoseMeters The starting pose estimate.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
    * @param kinematics A correctly-configured kinematics object for your drivetrain.
    * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-   *     meters and radians.
+   *     model's state estimates less. This matrix is in the form [x, y, theta, s_0, ... s_n]ᵀ, with
+   *     units in meters and radians, then meters.
    * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
    *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
-   *     is in the form [theta], with units in radians.
+   *     is in the form [theta, s_0, ... s_n], with units in radians followed by meters.
    * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
    *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
    *     theta]ᵀ, with units in meters and radians.
    * @param nominalDtSeconds The time in seconds between each robot loop.
    */
   public SwerveDrivePoseEstimator(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Nat<Outputs> outputs,
       Rotation2d gyroAngle,
       Pose2d initialPoseMeters,
+      SwerveModulePosition[] modulePositions,
       SwerveDriveKinematics kinematics,
-      Matrix<N3, N1> stateStdDevs,
-      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> localMeasurementStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs,
       double nominalDtSeconds) {
+    this.m_states = states;
+    this.m_inputs = inputs;
+    this.m_outputs = outputs;
+
+    if (states.getNum() != modulePositions.length + 3) {
+      throw new IllegalArgumentException(
+          String.format(
+              "Number of states (%s) must be 3 + "
+                  + "the number of modules provided in constructor (%s).",
+              states.getNum(), modulePositions.length));
+    }
+
+    if (inputs.getNum() != modulePositions.length + 3) {
+      throw new IllegalArgumentException(
+          String.format(
+              "Number of inputs (%s) must be 3 + "
+                  + "the number of modules provided in constructor (%s).",
+              inputs.getNum(), modulePositions.length));
+    }
+
+    if (outputs.getNum() != modulePositions.length + 1) {
+      throw new IllegalArgumentException(
+          String.format(
+              "Number of outputs (%s) must be 3 + "
+                  + "the number of modules provided in constructor (%s).",
+              outputs.getNum(), modulePositions.length));
+    }
+
     m_nominalDt = nominalDtSeconds;
 
     m_observer =
         new UnscentedKalmanFilter<>(
-            Nat.N3(),
-            Nat.N1(),
-            (x, u) -> u,
-            (x, u) -> x.extractRowVector(2),
+            states,
+            outputs,
+            (x, u) -> u.block(states.getNum(), 1, 0, 0),
+            (x, u) -> x.block(states.getNum() - 2, 1, 2, 0),
             stateStdDevs,
             localMeasurementStdDevs,
             AngleStatistics.angleMean(2),
@@ -146,7 +204,7 @@ public class SwerveDrivePoseEstimator {
                 Nat.N3(),
                 u,
                 y,
-                (x, u1) -> x,
+                (x, u1) -> x.block(3, 1, 0, 0),
                 m_visionContR,
                 AngleStatistics.angleMean(2),
                 AngleStatistics.angleResidual(2),
@@ -155,7 +213,18 @@ public class SwerveDrivePoseEstimator {
 
     m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
     m_previousAngle = initialPoseMeters.getRotation();
-    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
+
+    var poseVec = StateSpaceUtil.poseTo3dVector(initialPoseMeters);
+    Matrix<States, N1> xhat = new Matrix<States, N1>(states, Nat.N1());
+    xhat.set(0, 0, poseVec.get(0, 0));
+    xhat.set(1, 0, poseVec.get(1, 0));
+    xhat.set(2, 0, poseVec.get(2, 0));
+
+    for (int index = 3; index < states.getNum(); index++) {
+      xhat.set(index, 0, modulePositions[index - 3].distanceMeters);
+    }
+
+    m_observer.setXhat(xhat);
   }
 
   /**
@@ -179,13 +248,25 @@ public class SwerveDrivePoseEstimator {
    *
    * @param poseMeters The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
    */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Pose2d poseMeters, Rotation2d gyroAngle, SwerveModulePosition... modulePositions) {
     // Reset state estimate and error covariance
     m_observer.reset();
     m_poseBuffer.clear();
 
-    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+    var poseVec = StateSpaceUtil.poseTo3dVector(poseMeters);
+    Matrix<States, N1> xhat = new Matrix<States, N1>(m_states, Nat.N1());
+    xhat.set(0, 0, poseVec.get(0, 0));
+    xhat.set(1, 0, poseVec.get(1, 0));
+    xhat.set(2, 0, poseVec.get(2, 0));
+
+    for (int index = 3; index < m_states.getNum(); index++) {
+      xhat.set(index, 0, modulePositions[index - 3].distanceMeters);
+    }
+
+    m_observer.setXhat(xhat);
 
     m_prevTimeSeconds = -1;
 
@@ -225,7 +306,7 @@ public class SwerveDrivePoseEstimator {
     var sample = m_poseBuffer.getSample(timestampSeconds);
     if (sample.isPresent()) {
       m_visionCorrect.accept(
-          new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0),
+          new Matrix<Inputs, N1>(m_inputs, Nat.N1()),
           StateSpaceUtil.poseTo3dVector(
               getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
     }
@@ -271,10 +352,14 @@ public class SwerveDrivePoseEstimator {
    *
    * @param gyroAngle The current gyro angle.
    * @param moduleStates The current velocities and rotations of the swerve modules.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
    * @return The estimated pose of the robot in meters.
    */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  public Pose2d update(
+      Rotation2d gyroAngle,
+      SwerveModuleState[] moduleStates,
+      SwerveModulePosition[] modulePositions) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates, modulePositions);
   }
 
   /**
@@ -285,10 +370,14 @@ public class SwerveDrivePoseEstimator {
    * @param currentTimeSeconds Time at which this method was called, in seconds.
    * @param gyroAngle The current gyroscope angle.
    * @param moduleStates The current velocities and rotations of the swerve modules.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
    * @return The estimated pose of the robot in meters.
    */
   public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+      double currentTimeSeconds,
+      Rotation2d gyroAngle,
+      SwerveModuleState[] moduleStates,
+      SwerveModulePosition[] modulePositions) {
     double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
     m_prevTimeSeconds = currentTimeSeconds;
 
@@ -300,10 +389,23 @@ public class SwerveDrivePoseEstimator {
         new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
             .rotateBy(angle);
 
-    var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
+    var u = new Matrix<Inputs, N1>(m_inputs, Nat.N1());
+
+    u.set(0, 0, fieldRelativeVelocities.getX());
+    u.set(1, 0, fieldRelativeVelocities.getY());
+    u.set(2, 0, omega);
+    for (int index = 3; index < m_inputs.getNum(); index++) {
+      u.set(index, 0, moduleStates[index - 3].speedMetersPerSecond);
+    }
+
     m_previousAngle = angle;
 
-    var localY = VecBuilder.fill(angle.getRadians());
+    var localY = new Matrix<Outputs, N1>(m_outputs, Nat.N1());
+    localY.set(0, 0, angle.getRadians());
+    for (int index = 1; index < m_outputs.getNum(); index++) {
+      localY.set(index, 0, modulePositions[index - 1].distanceMeters);
+    }
+
     m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
     m_observer.predict(u, dt);
     m_observer.correct(u, localY);

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

@@ -8,6 +8,7 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import java.util.Arrays;
 import java.util.Collections;
 import org.ejml.simple.SimpleMatrix;
@@ -185,6 +186,35 @@ public class SwerveDriveKinematics {
         chassisSpeedsVector.get(2, 0));
   }
 
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the given module states.
+   * This method is often used for odometry -- determining the robot's position on the field using
+   * data from the real-world speed and angle of each module on the robot.
+   *
+   * @param wheelDeltas The latest change in position of the modules (as a SwerveModulePosition
+   *     type) as measured from respective encoders and gyros. The order of the swerve module states
+   *     should be same as passed into the constructor of this class.
+   * @return The resulting Twist2d.
+   */
+  public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) {
+    if (wheelDeltas.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+    var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = wheelDeltas[i];
+      moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
+      moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
+    }
+
+    var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix);
+    return new Twist2d(
+        chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
+  }
+
   /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    *

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

@@ -8,8 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.util.WPIUtilJNI;
 
 /**
  * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
@@ -22,24 +20,38 @@ import edu.wpi.first.util.WPIUtilJNI;
 public class SwerveDriveOdometry {
   private final SwerveDriveKinematics m_kinematics;
   private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
 
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
+  private final int m_numModules;
+  private SwerveModulePosition[] m_previousModulePositions;
 
   /**
    * Constructs a SwerveDriveOdometry object.
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    * @param initialPose The starting position of the robot on the field.
    */
   public SwerveDriveOdometry(
-      SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
+      SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose2d initialPose) {
     m_kinematics = kinematics;
     m_poseMeters = initialPose;
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
     m_previousAngle = initialPose.getRotation();
+    m_numModules = modulePositions.length;
+
+    m_previousModulePositions = new SwerveModulePosition[m_numModules];
+    for (int index = 0; index < m_numModules; index++) {
+      m_previousModulePositions[index] =
+          new SwerveModulePosition(
+              modulePositions[index].distanceMeters, modulePositions[index].angle);
+    }
+
     MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
   }
 
@@ -48,9 +60,13 @@ public class SwerveDriveOdometry {
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
+  public SwerveDriveOdometry(
+      SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition... modulePositions) {
+    this(kinematics, gyroAngle, modulePositions, new Pose2d());
   }
 
   /**
@@ -61,11 +77,24 @@ public class SwerveDriveOdometry {
    *
    * @param pose The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    */
-  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Pose2d pose, Rotation2d gyroAngle, SwerveModulePosition... modulePositions) {
+    if (modulePositions.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+
     m_poseMeters = pose;
     m_previousAngle = pose.getRotation();
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    for (int index = 0; index < m_numModules; index++) {
+      m_previousModulePositions[index] =
+          new SwerveModulePosition(
+              modulePositions[index].distanceMeters, modulePositions[index].angle);
+    }
   }
 
   /**
@@ -77,40 +106,6 @@ public class SwerveDriveOdometry {
     return m_poseMeters;
   }
 
-  /**
-   * 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.
-   *
-   * @param currentTimeSeconds The current time in seconds.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide the states in the
-   *     same order in which you instantiated your SwerveDriveKinematics.
-   * @return The new pose of the robot.
-   */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(
-                chassisState.vxMetersPerSecond * period,
-                chassisState.vyMetersPerSecond * period,
-                angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
-  }
-
   /**
    * Updates the robot's position on the field using forward kinematics and integration of the pose
    * over time. This method automatically calculates the current time to calculate period
@@ -119,11 +114,40 @@ public class SwerveDriveOdometry {
    * rate that is calculated from forward kinematics.
    *
    * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide the states in the
-   *     same order in which you instantiated your SwerveDriveKinematics.
+   * @param modulePositions The current position of all swerve modules. Please provide the positions
+   *     in the same order in which you instantiated your SwerveDriveKinematics.
    * @return The new pose of the robot.
    */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition... modulePositions) {
+    if (modulePositions.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+
+    var moduleDeltas = new SwerveModulePosition[m_numModules];
+    for (int index = 0; index < m_numModules; index++) {
+      var current = modulePositions[index];
+      var previous = m_previousModulePositions[index];
+
+      moduleDeltas[index] =
+          new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
+    }
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var twist = m_kinematics.toTwist2d(moduleDeltas);
+    twist.dtheta = angle.minus(m_previousAngle).getRadians();
+
+    var newPose = m_poseMeters.exp(twist);
+
+    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;
   }
 }

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

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.Objects;
+
+/** Represents the state of one swerve module. */
+public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+  /** Distance measured by the wheel of the module. */
+  public double distanceMeters;
+
+  /** Angle of the module. */
+  public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+  /** Constructs a SwerveModulePosition with zeros for speed and angle. */
+  public SwerveModulePosition() {}
+
+  /**
+   * Constructs a SwerveModulePosition.
+   *
+   * @param distanceMeters The distance measured by the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
+    this.distanceMeters = distanceMeters;
+    this.angle = angle;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SwerveModulePosition) {
+      return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(distanceMeters);
+  }
+
+  /**
+   * Compares two swerve module positions. One swerve module is "greater" than the other if its
+   * speed is higher than the other.
+   *
+   * @param other The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  public int compareTo(SwerveModulePosition other) {
+    return Double.compare(this.distanceMeters, other.distanceMeters);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "SwerveModulePosition(Distance: %.2f m/s, Angle: %s)", distanceMeters, angle);
+  }
+}