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

2026-06-23 01:21:42 +00:00 · 2022-10-25 15:28:36 -04:00
parent fe400f68c5
commit 4170ec6107
35 changed files with 1508 additions and 277 deletions
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -6,12 +6,16 @@ package edu.wpi.first.math.estimator;

 import static org.junit.jupiter.api.Assertions.assertEquals;

+import edu.wpi.first.math.Nat;
 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.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.numbers.N7;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
@@ -20,20 +24,30 @@ import org.junit.jupiter.api.Test;

 class SwerveDrivePoseEstimatorTest {
  @Test
-  void testAccuracy() {
+  void testAccuracyFacingTrajectory() {
    var kinematics =
        new SwerveDriveKinematics(
            new Translation2d(1, 1),
            new Translation2d(1, -1),
            new Translation2d(-1, -1),
            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
    var estimator =
-        new SwerveDrivePoseEstimator(
+        new SwerveDrivePoseEstimator<N7, N7, N5>(
+            Nat.N7(),
+            Nat.N7(),
+            Nat.N5(),
            new Rotation2d(),
            new Pose2d(),
+            new SwerveModulePosition[] {fl, fr, bl, br},
            kinematics,
-            VecBuilder.fill(0.1, 0.1, 0.1),
-            VecBuilder.fill(0.005),
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),
            VecBuilder.fill(0.1, 0.1, 0.1));

    var trajectory =
@@ -89,6 +103,16 @@ class SwerveDrivePoseEstimatorTest {
        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
      }

+      fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt;
+      fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt;
+      bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt;
+      br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt;
+
+      fl.angle = moduleStates[0].angle;
+      fr.angle = moduleStates[1].angle;
+      bl.angle = moduleStates[2].angle;
+      br.angle = moduleStates[3].angle;
+
      var xHat =
          estimator.updateWithTime(
              t,
@@ -96,7 +120,129 @@ class SwerveDrivePoseEstimatorTest {
                  .poseMeters
                  .getRotation()
                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
-              moduleStates);
+              moduleStates,
+              new SwerveModulePosition[] {fl, fr, bl, br});
+
+      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 SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator<N7, N7, N5>(
+            Nat.N7(),
+            Nat.N7(),
+            Nat.N5(),
+            new Rotation2d(),
+            new Pose2d(),
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(4915);
+
+    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 moduleStates =
+          kinematics.toSwerveModuleStates(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getCos(),
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getSin(),
+                  0.0));
+      for (var moduleState : moduleStates) {
+        moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+      }
+
+      fl.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      fr.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      bl.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      br.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+      fl.angle = groundTruthState.poseMeters.getRotation();
+      fr.angle = groundTruthState.poseMeters.getRotation();
+      bl.angle = groundTruthState.poseMeters.getRotation();
+      br.angle = groundTruthState.poseMeters.getRotation();
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              new Rotation2d(rand.nextGaussian() * 0.05),
+              moduleStates,
+              new SwerveModulePosition[] {fl, fr, bl, br});

      double error =
          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
@@ -41,15 +41,27 @@ class SwerveDriveKinematicsTest {

  @Test
  void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
-    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(0.0));
    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);

    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
  }

+  @Test
+  void testStraightLineForwardKinematicsWithDeltas() {
+    // test forward kinematics going in a straight line
+    SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(0.0));
+    var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
+
+    assertAll(
+        () -> assertEquals(5.0, twist.dx, kEpsilon),
+        () -> assertEquals(0.0, twist.dy, kEpsilon),
+        () -> assertEquals(0.0, twist.dtheta, kEpsilon));
+  }
+
  @Test
  void testStraightStrafeInverseKinematics() {
    ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
@@ -77,6 +89,17 @@ class SwerveDriveKinematicsTest {
        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
  }

+  @Test
+  void testStraightStrafeForwardKinematicsWithDeltas() {
+    SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(90.0));
+    var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
+
+    assertAll(
+        () -> assertEquals(0.0, twist.dx, kEpsilon),
+        () -> assertEquals(5.0, twist.dy, kEpsilon),
+        () -> assertEquals(0.0, twist.dtheta, kEpsilon));
+  }
+
  @Test
  void testConserveWheelAngle() {
    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
@@ -134,6 +157,21 @@ class SwerveDriveKinematicsTest {
        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testTurnInPlaceForwardKinematicsWithDeltas() {
+    SwerveModulePosition flDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(135));
+    SwerveModulePosition frDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(45));
+    SwerveModulePosition blDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-135));
+    SwerveModulePosition brDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-45));
+
+    var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+    assertAll(
+        () -> assertEquals(0.0, twist.dx, kEpsilon),
+        () -> assertEquals(0.0, twist.dy, kEpsilon),
+        () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+  }
+
  @Test
  void testOffCenterCORRotationInverseKinematics() {
    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
@@ -183,6 +221,30 @@ class SwerveDriveKinematicsTest {
        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testOffCenterCORRotationForwardKinematicsWithDeltas() {
+    SwerveModulePosition flDelta = new SwerveModulePosition(0.0, Rotation2d.fromDegrees(0.0));
+    SwerveModulePosition frDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(0.0));
+    SwerveModulePosition blDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(-90));
+    SwerveModulePosition brDelta = new SwerveModulePosition(213.258, Rotation2d.fromDegrees(-45));
+
+    var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+    /*
+    We already know that our omega should be 2π from the previous test. Next, we need to determine
+    the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
+    we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
+    a full revolution about the center of revolution once every second. Therefore, the center of
+    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
+    1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+    */
+
+    assertAll(
+        () -> assertEquals(75.398, twist.dx, 0.1),
+        () -> assertEquals(-75.398, twist.dy, 0.1),
+        () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+  }
+
  private void assertModuleState(
      SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) {
    assertAll(
@@ -247,6 +309,30 @@ class SwerveDriveKinematicsTest {
        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1));
  }

+  @Test
+  void testOffCenterCORRotationAndTranslationForwardKinematicsWithDeltas() {
+    SwerveModulePosition flDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-140.19));
+    SwerveModulePosition frDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-39.81));
+    SwerveModulePosition blDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-109.44));
+    SwerveModulePosition brDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-70.56));
+
+    var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+    /*
+    From equation (13.17), we know that chassis motion is th dot product of the
+    pseudoinverse of the inverseKinematics matrix (with the center of rotation at
+    (0,0) -- we don't want the motion of the center of rotation, we want it of
+    the center of the robot). These above SwerveModuleStates are known to be from
+    a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
+    calculated using Numpy's linalg.pinv function.
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, twist.dx, 0.1),
+        () -> assertEquals(-33.0, twist.dy, 0.1),
+        () -> assertEquals(1.5, twist.dtheta, 0.1));
+  }
+
  @Test
  void testDesaturate() {
    SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.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 SwerveDriveOdometryTest {
@@ -18,30 +22,31 @@ class SwerveDriveOdometryTest {
  private final Translation2d m_bl = new Translation2d(-12, 12);
  private final Translation2d m_br = new Translation2d(-12, -12);

+  private final SwerveModulePosition zero = new SwerveModulePosition();
+
  private final SwerveDriveKinematics m_kinematics =
      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);

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

  @Test
  void testTwoIterations() {
    // 5 units/sec  in the x axis (forward)
-    final SwerveModuleState[] wheelSpeeds = {
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0))
+    final SwerveModulePosition[] wheelDeltas = {
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0))
    };

-    m_odometry.updateWithTime(
-        0.0,
+    m_odometry.update(
        new Rotation2d(),
-        new SwerveModuleState(),
-        new SwerveModuleState(),
-        new SwerveModuleState(),
-        new SwerveModuleState());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition());
+    var pose = m_odometry.update(new Rotation2d(), wheelDeltas);

    assertAll(
        () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
@@ -57,16 +62,16 @@ class SwerveDriveOdometryTest {
    //        Module 2: speed 18.84955592153876 angle -90.0
    //        Module 3: speed 42.14888838624436 angle -26.565051177077986

-    final SwerveModuleState[] wheelSpeeds = {
-      new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
-      new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
-      new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
-      new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
+    final SwerveModulePosition[] wheelDeltas = {
+      new SwerveModulePosition(18.85, Rotation2d.fromDegrees(90.0)),
+      new SwerveModulePosition(42.15, Rotation2d.fromDegrees(26.565)),
+      new SwerveModulePosition(18.85, Rotation2d.fromDegrees(-90)),
+      new SwerveModulePosition(42.15, Rotation2d.fromDegrees(-26.565))
    };
-    final var zero = new SwerveModuleState();
+    final var zero = new SwerveModulePosition();

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

    assertAll(
        () -> assertEquals(12.0, pose.getX(), 0.01),
@@ -78,15 +83,169 @@ class SwerveDriveOdometryTest {
  void testGyroAngleReset() {
    var gyro = Rotation2d.fromDegrees(90.0);
    var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var state = new SwerveModuleState();
-    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
-    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
-    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+    m_odometry.resetPosition(
+        new Pose2d(new Translation2d(), fieldAngle), gyro, zero, zero, zero, zero);
+    var delta = new SwerveModulePosition();
+    m_odometry.update(gyro, delta, delta, delta, delta);
+    delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));
+    var pose = m_odometry.update(gyro, delta, delta, delta, delta);

    assertAll(
        () -> assertEquals(1.0, pose.getX(), 0.1),
        () -> assertEquals(0.00, pose.getY(), 0.1),
        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
  }
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+    var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero);
+
+    SwerveModulePosition fl = new SwerveModulePosition();
+    SwerveModulePosition fr = new SwerveModulePosition();
+    SwerveModulePosition bl = new SwerveModulePosition();
+    SwerveModulePosition br = new SwerveModulePosition();
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      var moduleStates =
+          kinematics.toSwerveModuleStates(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond,
+                  0.0,
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.curvatureRadPerMeter));
+      for (var moduleState : moduleStates) {
+        moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+      }
+
+      fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt;
+      fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt;
+      bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt;
+      br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt;
+
+      fl.angle = moduleStates[0].angle;
+      fr.angle = moduleStates[1].angle;
+      bl.angle = moduleStates[2].angle;
+      br.angle = moduleStates[3].angle;
+
+      var xHat =
+          odometry.update(
+              groundTruthState
+                  .poseMeters
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              fl,
+              fr,
+              bl,
+              br);
+
+      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 SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+    var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero);
+
+    SwerveModulePosition fl = new SwerveModulePosition();
+    SwerveModulePosition fr = new SwerveModulePosition();
+    SwerveModulePosition bl = new SwerveModulePosition();
+    SwerveModulePosition br = new SwerveModulePosition();
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      fl.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      fr.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      bl.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      br.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+      fl.angle = groundTruthState.poseMeters.getRotation();
+      fr.angle = groundTruthState.poseMeters.getRotation();
+      bl.angle = groundTruthState.poseMeters.getRotation();
+      br.angle = groundTruthState.poseMeters.getRotation();
+
+      var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), fl, fr, bl, br);
+
+      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.06, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+  }
 }