[wpimath] Position Delta Odometry for Mecanum (#4514)

2026-06-21 01:01:43 +00:00 · 2022-10-25 15:28:59 -04:00
parent 4170ec6107
commit 901fc555f4
28 changed files with 1222 additions and 235 deletions
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
@@ -20,19 +21,22 @@ import org.junit.jupiter.api.Test;

 class MecanumDrivePoseEstimatorTest {
  @Test
-  void testAccuracy() {
+  void testAccuracyFacingTrajectory() {
    var kinematics =
        new MecanumDriveKinematics(
            new Translation2d(1, 1), new Translation2d(1, -1),
            new Translation2d(-1, -1), new Translation2d(-1, 1));

+    var wheelPositions = new MecanumDriveWheelPositions();
+
    var estimator =
        new MecanumDrivePoseEstimator(
            new Rotation2d(),
            new Pose2d(),
+            wheelPositions,
            kinematics,
-            VecBuilder.fill(0.1, 0.1, 0.1),
-            VecBuilder.fill(0.05),
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
            VecBuilder.fill(0.1, 0.1, 0.1));

    var trajectory =
@@ -90,6 +94,11 @@ class MecanumDrivePoseEstimatorTest {
      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;

+      wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+      wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+      wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+      wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
      var xHat =
          estimator.updateWithTime(
              t,
@@ -97,7 +106,107 @@ class MecanumDrivePoseEstimatorTest {
                  .poseMeters
                  .getRotation()
                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
-              wheelSpeeds);
+              wheelSpeeds,
+              wheelPositions);
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+  }
+
+  @Test
+  void testAccuracyFacingXAxis() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            new Rotation2d(),
+            new Pose2d(),
+            wheelPositions,
+            kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      if (lastVisionUpdateTime + visionUpdateRate < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+
+        lastVisionPose =
+            new Pose2d(
+                new Translation2d(
+                    groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
+                    groundTruthState.poseMeters.getTranslation().getY()
+                        + rand.nextGaussian() * 0.1),
+                new Rotation2d(rand.nextGaussian() * 0.1)
+                    .plus(groundTruthState.poseMeters.getRotation()));
+
+        lastVisionUpdateTime = t;
+      }
+
+      var wheelSpeeds =
+          kinematics.toWheelSpeeds(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getCos(),
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getSin(),
+                  0));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+      wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+      wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+      wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
+      var xHat =
+          estimator.updateWithTime(
+              t, new Rotation2d(rand.nextGaussian() * 0.05), wheelSpeeds, wheelPositions);

      double error =
          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
@@ -44,6 +44,17 @@ class MecanumDriveKinematicsTest {
        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testStraightLineForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(3.536, twist.dx, 0.1),
+        () -> assertEquals(0, twist.dy, 0.1),
+        () -> assertEquals(0, twist.dtheta, 0.1));
+  }
+
  @Test
  void testStrafeInverseKinematics() {
    ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
@@ -67,6 +78,17 @@ class MecanumDriveKinematicsTest {
        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testStrafeForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(-2.828427, 2.828427, 2.828427, -2.828427);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(0, twist.dx, 0.1),
+        () -> assertEquals(2.8284, twist.dy, 0.1),
+        () -> assertEquals(0, twist.dtheta, 0.1));
+  }
+
  @Test
  void testRotationInverseKinematics() {
    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
@@ -90,6 +112,17 @@ class MecanumDriveKinematicsTest {
        () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testRotationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(-150.79645, 150.79645, -150.79645, 150.79645);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(0, twist.dx, 0.1),
+        () -> assertEquals(0, twist.dy, 0.1),
+        () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+  }
+
  @Test
  void testMixedTranslationRotationInverseKinematics() {
    ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
@@ -113,6 +146,17 @@ class MecanumDriveKinematicsTest {
        () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testMixedTranslationRotationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(-17.677670, 20.51, -13.44, 16.26);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(1.413, twist.dx, 0.1),
+        () -> assertEquals(2.122, twist.dy, 0.1),
+        () -> assertEquals(0.707, twist.dtheta, 0.1));
+  }
+
  @Test
  void testOffCenterRotationInverseKinematics() {
    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
@@ -136,6 +180,17 @@ class MecanumDriveKinematicsTest {
        () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testOffCenterRotationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(0, 16.971, -16.971, 33.941);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(8.48525, twist.dx, 0.1),
+        () -> assertEquals(-8.48525, twist.dy, 0.1),
+        () -> assertEquals(0.707, twist.dtheta, 0.1));
+  }
+
  @Test
  void testOffCenterTranslationRotationInverseKinematics() {
    ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
@@ -159,6 +214,17 @@ class MecanumDriveKinematicsTest {
        () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testOffCenterRotationTranslationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(2.12, 21.92, -12.02, 36.06);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(12.02, twist.dx, 0.1),
+        () -> assertEquals(-7.07, twist.dy, 0.1),
+        () -> assertEquals(0.707, twist.dtheta, 0.1));
+  }
+
  @Test
  void testDesaturate() {
    var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
@@ -10,6 +10,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 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.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
 import org.junit.jupiter.api.Test;

 class MecanumDriveOdometryTest {
@@ -21,15 +25,19 @@ class MecanumDriveOdometryTest {
  private final MecanumDriveKinematics m_kinematics =
      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);

+  private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions();
+
  private final MecanumDriveOdometry m_odometry =
-      new MecanumDriveOdometry(m_kinematics, new Rotation2d());
+      new MecanumDriveOdometry(m_kinematics, new Rotation2d(), zero);

  @Test
  void testMultipleConsecutiveUpdates() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);

-    m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
-    var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+    m_odometry.resetPosition(new Pose2d(), new Rotation2d(), wheelPositions);
+
+    m_odometry.update(new Rotation2d(), wheelPositions);
+    var secondPose = m_odometry.update(new Rotation2d(), wheelPositions);

    assertAll(
        () -> assertEquals(secondPose.getX(), 0.0, 0.01),
@@ -40,10 +48,11 @@ class MecanumDriveOdometryTest {
  @Test
  void testTwoIterations() {
    // 5 units/sec  in the x axis (forward)
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536);
+    m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());

-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+    m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
+    var pose = m_odometry.update(new Rotation2d(), wheelPositions);

    assertAll(
        () -> assertEquals(0.3536, pose.getX(), 0.01),
@@ -55,10 +64,11 @@ class MecanumDriveOdometryTest {
  void test90degreeTurn() {
    // This is a 90 degree turn about the point between front left and rear left wheels
    // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
+    final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986);
+    m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());

-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+    m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
+    final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions);

    assertAll(
        () -> assertEquals(8.4855, pose.getX(), 0.01),
@@ -70,14 +80,188 @@ class MecanumDriveOdometryTest {
  void testGyroAngleReset() {
    var gyro = Rotation2d.fromDegrees(90.0);
    var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+    m_odometry.resetPosition(
+        new Pose2d(new Translation2d(), fieldAngle), gyro, new MecanumDriveWheelPositions());
+    var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
+    m_odometry.update(gyro, new MecanumDriveWheelPositions());
+    var pose = m_odometry.update(gyro, speeds);

    assertAll(
        () -> assertEquals(3.536, pose.getX(), 0.1),
        () -> assertEquals(0.0, pose.getY(), 0.1),
        () -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1));
  }
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var odometry =
+        new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    double odometryDistanceTravelled = 0;
+    double trajectoryDistanceTravelled = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      trajectoryDistanceTravelled +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+      var wheelSpeeds =
+          kinematics.toWheelSpeeds(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond,
+                  0,
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.curvatureRadPerMeter));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+      wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+      wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+      wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
+      var lastPose = odometry.getPoseMeters();
+
+      var xHat =
+          odometry.update(
+              groundTruthState
+                  .poseMeters
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              wheelPositions);
+
+      odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+    assertEquals(
+        1.0,
+        odometryDistanceTravelled / trajectoryDistanceTravelled,
+        0.05,
+        "Incorrect distance travelled");
+  }
+
+  @Test
+  void testAccuracyFacingXAxis() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var odometry =
+        new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    double odometryDistanceTravelled = 0;
+    double trajectoryDistanceTravelled = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      trajectoryDistanceTravelled +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+      var wheelSpeeds =
+          kinematics.toWheelSpeeds(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getCos(),
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getSin(),
+                  0));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+      wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+      wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+      wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
+      var lastPose = odometry.getPoseMeters();
+
+      var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions);
+
+      odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+    assertEquals(
+        1.0,
+        odometryDistanceTravelled / trajectoryDistanceTravelled,
+        0.05,
+        "Incorrect distance travelled");
+  }
 }