[wpimath] Fix ResetTranslation and ResetRotation in PoseEstimator and PoseEstimator3d causing the robot to teleport (#8285)

Fixes https://github.com/wpilibsuite/allwpilib/issues/8284. If we have vision updates at the time of the `Reset*` call, we can correct the translation/rotation of the new odometry pose by adding a new vision update where: - `ResetTranslation`: the translation is hard-coded to the new translation, and the rotation components are set to those of the latest vision update (prior to clearing the map). - `ResetRotation`: the rotation is hard-coded to the new rotation, and the translation components are set to those of the latest vision update (prior to clearing the map).
2026-06-19 00:41:43 +00:00 · 2025-12-13 18:53:53 -05:00
parent baa6379267
commit 3dc334c1ee
8 changed files with 275 additions and 24 deletions
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -49,7 +49,9 @@ public class PoseEstimator<T> {
      TimeInterpolatableBuffer.createBuffer(kBufferDuration);
  // Maps timestamps to vision updates
  // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
-  // been no vision measurements after the last reset
+  // been no vision measurements after the last reset. May contain one entry while
+  // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
+  // ResetRotation/ResetTranslation.
  private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();

  private Pose2d m_poseEstimate;
@@ -147,9 +149,22 @@ public class PoseEstimator<T> {
   */
  public void resetTranslation(Translation2d translation) {
    m_odometry.resetTranslation(translation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
    m_odometryPoseBuffer.clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose rotation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose2d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
+              new Pose2d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
  }

  /**
@@ -159,9 +174,22 @@ public class PoseEstimator<T> {
   */
  public void resetRotation(Rotation2d rotation) {
    m_odometry.resetRotation(rotation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
    m_odometryPoseBuffer.clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose translation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose2d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
+              new Pose2d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
  }

  /**
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
@@ -57,7 +57,9 @@ public class PoseEstimator3d<T> {
      TimeInterpolatableBuffer.createBuffer(kBufferDuration);
  // Maps timestamps to vision updates
  // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
-  // been no vision measurements after the last reset
+  // been no vision measurements after the last reset. May contain one entry while
+  // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
+  // ResetRotation/ResetTranslation.
  private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();

  private Pose3d m_poseEstimate;
@@ -159,9 +161,22 @@ public class PoseEstimator3d<T> {
   */
  public void resetTranslation(Translation3d translation) {
    m_odometry.resetTranslation(translation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
    m_odometryPoseBuffer.clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose rotation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose3d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
+              new Pose3d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
  }

  /**
@@ -171,9 +186,22 @@ public class PoseEstimator3d<T> {
   */
  public void resetRotation(Rotation3d rotation) {
    m_odometry.resetRotation(rotation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
    m_odometryPoseBuffer.clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose translation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose3d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
+              new Pose3d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
  }

  /**
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -139,24 +139,70 @@ class WPILIB_DLLEXPORT PoseEstimator {
   *
   * @param translation The pose to translation to.
   */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
  void ResetTranslation(const Translation2d& translation) {
    m_odometry.ResetTranslation(translation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
    m_odometryPoseBuffer.Clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose rotation
+      const VisionUpdate visionUpdate{
+          Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+          Pose2d{translation,
+                 latestVisionUpdate->second.odometryPose.Rotation()}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
  }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)

  /**
   * Resets the robot's rotation.
   *
   * @param rotation The rotation to reset to.
   */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
  void ResetRotation(const Rotation2d& rotation) {
    m_odometry.ResetRotation(rotation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
    m_odometryPoseBuffer.Clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose translation
+      const VisionUpdate visionUpdate{
+          Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+          Pose2d{latestVisionUpdate->second.odometryPose.Translation(),
+                 rotation}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
  }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)

  /**
   * Gets the estimated robot pose.
@@ -436,7 +482,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
  TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};
  // Maps timestamps to vision updates
  // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
-  // unless there have been no vision measurements after the last reset
+  // unless there have been no vision measurements after the last reset. May
+  // contain one entry while m_odometryPoseBuffer is empty to correct for
+  // translation/rotation after a call to ResetRotation/ResetTranslation.
  std::map<units::second_t, VisionUpdate> m_visionUpdates;

  Pose2d m_poseEstimate;
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -148,24 +148,70 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
   *
   * @param translation The pose to translation to.
   */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
  void ResetTranslation(const Translation3d& translation) {
    m_odometry.ResetTranslation(translation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
    m_odometryPoseBuffer.Clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose rotation
+      const VisionUpdate visionUpdate{
+          Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+          Pose3d{translation,
+                 latestVisionUpdate->second.odometryPose.Rotation()}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
  }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)

  /**
   * Resets the robot's rotation.
   *
   * @param rotation The rotation to reset to.
   */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
  void ResetRotation(const Rotation3d& rotation) {
    m_odometry.ResetRotation(rotation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
    m_odometryPoseBuffer.Clear();
    m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose translation
+      const VisionUpdate visionUpdate{
+          Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+          Pose3d{latestVisionUpdate->second.odometryPose.Translation(),
+                 rotation}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
  }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)

  /**
   * Gets the estimated robot pose.
@@ -451,7 +497,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
  TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
  // Maps timestamps to vision updates
  // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
-  // unless there have been no vision measurements after the last reset
+  // unless there have been no vision measurements after the last reset. May
+  // contain one entry while m_odometryPoseBuffer is empty to correct for
+  // translation/rotation after a call to ResetRotation/ResetTranslation.
  std::map<units::second_t, VisionUpdate> m_visionUpdates;

  Pose3d m_poseEstimate;
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;

+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -509,11 +510,23 @@ class SwerveDrivePoseEstimator3dTest {
        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));

+    // Add a vision measurement with a different translation
+    estimator.addVisionMeasurement(
+        new Pose3d(3, 0, 0, Rotation3d.kZero), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+
    // Test reset rotation
    estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2));

    assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -533,7 +546,7 @@ class SwerveDrivePoseEstimator3dTest {
    }

    assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -542,6 +555,22 @@ class SwerveDrivePoseEstimator3dTest {
            assertEquals(
                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));

+    // Add a vision measurement with a different rotation
+    estimator.addVisionMeasurement(
+        new Pose3d(2.5, 1, 0, new Rotation3d(Rotation2d.kPi)), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                kEpsilon));
+
    // Test reset translation
    estimator.resetTranslation(new Translation3d(-1, -1, -1));

@@ -553,7 +582,9 @@ class SwerveDrivePoseEstimator3dTest {
        () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
        () ->
            assertEquals(
-                Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                kEpsilon));

    // Test reset pose
    estimator.resetPose(Pose3d.kZero);
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;

+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -481,11 +482,21 @@ class SwerveDrivePoseEstimatorTest {
        () ->
            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));

+    // Add a vision measurement with a different translation
+    estimator.addVisionMeasurement(
+        new Pose2d(3, 0, Rotation2d.kZero), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
    // Test reset rotation
    estimator.resetRotation(Rotation2d.kCCW_Pi_2);

    assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
        () ->
            assertEquals(
@@ -504,7 +515,7 @@ class SwerveDrivePoseEstimatorTest {
    }

    assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
        () ->
            assertEquals(
@@ -512,6 +523,19 @@ class SwerveDrivePoseEstimatorTest {
                estimator.getEstimatedPosition().getRotation().getRadians(),
                kEpsilon));

+    // Add a vision measurement with a different rotation
+    estimator.addVisionMeasurement(
+        new Pose2d(2.5, 1, Rotation2d.kPi), MathSharedStore.getTimestamp());
+
+    assertAll(
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
    // Test reset translation
    estimator.resetTranslation(new Translation2d(-1, -1));

@@ -520,7 +544,7 @@ class SwerveDrivePoseEstimatorTest {
        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
        () ->
            assertEquals(
-                Math.PI / 2,
+                Math.PI * 3.0 / 4,
                estimator.getEstimatedPosition().getRotation().getRadians(),
                kEpsilon));

--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
@@ -453,10 +453,21 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());

+  // Add a vision measurement with a different translation
+  estimator.AddVisionMeasurement(frc::Pose3d(3_m, 0_m, 0_m, frc::Rotation3d{}),
+                                 wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
+
  // Test reset rotation
  estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg});

-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -471,7 +482,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
                                         modulePosition, modulePosition});
  }

-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -479,6 +490,19 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
  EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
                   estimator.GetEstimatedPosition().Rotation().Z().value());

+  // Add a vision measurement with a different rotation
+  estimator.AddVisionMeasurement(
+      frc::Pose3d(2.5_m, 1_m, 0_m, frc::Rotation3d{frc::Rotation2d{180_deg}}),
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
+                   estimator.GetEstimatedPosition().Rotation().Z().value());
+
  // Test reset translation
  estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});

@@ -487,7 +511,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
  EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
-  EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
+  EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
                   estimator.GetEstimatedPosition().Rotation().Z().value());

  // Test reset pose
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -430,10 +430,19 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
  EXPECT_DOUBLE_EQ(
      0, estimator.GetEstimatedPosition().Rotation().Radians().value());

+  // Add a vision measurement with a different translation
+  estimator.AddVisionMeasurement(frc::Pose2d{3_m, 0_m, frc::Rotation2d{}},
+                                 wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(
+      0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
  // Test reset rotation
  estimator.ResetRotation(frc::Rotation2d{90_deg});

-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
  EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
  EXPECT_DOUBLE_EQ(
      std::numbers::pi / 2,
@@ -446,19 +455,30 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
                                         modulePosition, modulePosition});
  }

-  EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
  EXPECT_DOUBLE_EQ(
      std::numbers::pi / 2,
      estimator.GetEstimatedPosition().Rotation().Radians().value());

+  // Add a vision measurement with a different rotation
+  estimator.AddVisionMeasurement(
+      frc::Pose2d{2.5_m, 1_m, frc::Rotation2d{180_deg}},
+      wpi::math::MathSharedStore::GetTimestamp());
+
+  EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
+  EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_DOUBLE_EQ(
+      std::numbers::pi * 3.0 / 4,
+      estimator.GetEstimatedPosition().Rotation().Radians().value());
+
  // Test reset translation
  estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});

  EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
  EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
  EXPECT_DOUBLE_EQ(
-      std::numbers::pi / 2,
+      std::numbers::pi * 3.0 / 4,
      estimator.GetEstimatedPosition().Rotation().Radians().value());

  // Test reset pose