From 2e45d07a339c463d2e9bb16f096a728b10fb8c98 Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Fri, 10 Oct 2025 00:05:00 -0400
Subject: [PATCH 1/8] Fix `ResetTranslation` and `ResetRotation` in
 `PoseEstimator` and `PoseEstimator3d`

---
 .../first/math/estimator/PoseEstimator.java   | 30 +++++++++++++++--
 .../first/math/estimator/PoseEstimator3d.java | 30 +++++++++++++++--
 .../include/frc/estimator/PoseEstimator.h     | 32 +++++++++++++++++--
 .../include/frc/estimator/PoseEstimator3d.h   | 32 +++++++++++++++++--
 4 files changed, 116 insertions(+), 8 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
index 1f00cb7b187..159c2b3ed9b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -145,9 +145,22 @@ public void resetPose(Pose2d pose) {
    */
   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();
+    }
   }
 
   /**
@@ -157,9 +170,22 @@ public void resetTranslation(Translation2d translation) {
    */
   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();
+    }
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
index 9ce96f0243b..abbaaa572e6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
@@ -157,9 +157,22 @@ public void resetPose(Pose3d pose) {
    */
   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();
+    }
   }
 
   /**
@@ -169,9 +182,22 @@ public void resetTranslation(Translation3d translation) {
    */
   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();
+    }
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index 9c3bc4d6dee..34f3954e753 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -138,9 +138,23 @@ class WPILIB_DLLEXPORT PoseEstimator {
    */
   void ResetTranslation(const Translation2d& translation) {
     m_odometry.ResetTranslation(translation);
+
+    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
+      VisionUpdate const visionUpdate{
+        Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+        Pose2d{translation, latestVisionUpdate->second.odometryPose.Rotation()}
+      };
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
 
   /**
@@ -150,9 +164,23 @@ class WPILIB_DLLEXPORT PoseEstimator {
    */
   void ResetRotation(const Rotation2d& rotation) {
     m_odometry.ResetRotation(rotation);
+
+    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
+      VisionUpdate const visionUpdate{
+        Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+        Pose2d{latestVisionUpdate->second.odometryPose.Translation(), rotation}
+      };
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
index bc8bdb08a66..5a05a6fe848 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -147,9 +147,23 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
    */
   void ResetTranslation(const Translation3d& translation) {
     m_odometry.ResetTranslation(translation);
+
+    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
+      VisionUpdate const visionUpdate{
+        Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+        Pose3d{translation, latestVisionUpdate->second.odometryPose.Rotation()}
+      };
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
 
   /**
@@ -159,9 +173,23 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
    */
   void ResetRotation(const Rotation3d& rotation) {
     m_odometry.ResetRotation(rotation);
+
+    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
+      VisionUpdate const visionUpdate{
+        Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+        Pose3d{latestVisionUpdate->second.odometryPose.Translation(), rotation}
+      };
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
 
   /**

From 236400eaf0c9c16412b9ff41b4dc59c0cf9bc873 Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Fri, 10 Oct 2025 00:17:09 -0400
Subject: [PATCH 2/8] Fix typo

---
 wpimath/src/main/native/include/frc/estimator/PoseEstimator.h | 4 ++--
 .../src/main/native/include/frc/estimator/PoseEstimator3d.h   | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index 34f3954e753..b472868a182 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -151,7 +151,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
         Pose2d{translation, latestVisionUpdate->second.odometryPose.Rotation()}
       };
       m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
-      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
     } else {
       m_poseEstimate = m_odometry.GetPose();
     }
@@ -177,7 +177,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
         Pose2d{latestVisionUpdate->second.odometryPose.Translation(), rotation}
       };
       m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
-      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
     } else {
       m_poseEstimate = m_odometry.GetPose();
     }
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
index 5a05a6fe848..c6f43fef651 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -160,7 +160,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
         Pose3d{translation, latestVisionUpdate->second.odometryPose.Rotation()}
       };
       m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
-      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
     } else {
       m_poseEstimate = m_odometry.GetPose();
     }
@@ -186,7 +186,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
         Pose3d{latestVisionUpdate->second.odometryPose.Translation(), rotation}
       };
       m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
-      m_poseEstimate = visionUpdate.Compensate(m_odometry.Pose());
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
     } else {
       m_poseEstimate = m_odometry.GetPose();
     }

From 4e3ab9981188a4a3e34918cce5dab2360d1abedd Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Fri, 10 Oct 2025 00:42:02 -0400
Subject: [PATCH 3/8] Attempt to fix maybe-uninitialized warning on Linux

---
 .../first/math/estimator/PoseEstimator.java   | 16 +++++++--------
 .../first/math/estimator/PoseEstimator3d.java | 16 +++++++--------
 .../include/frc/estimator/PoseEstimator.h     | 20 +++++++++++--------
 .../include/frc/estimator/PoseEstimator3d.h   | 20 +++++++++++--------
 4 files changed, 40 insertions(+), 32 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
index 159c2b3ed9b..49cf94a5f8c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -152,10 +152,10 @@ public void resetTranslation(Translation2d translation) {
 
     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())
-      );
+      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 {
@@ -177,10 +177,10 @@ public void resetRotation(Rotation2d rotation) {
 
     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)
-      );
+      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 {
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
index abbaaa572e6..4d7ede2b239 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
@@ -164,10 +164,10 @@ public void resetTranslation(Translation3d translation) {
 
     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())
-      );
+      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 {
@@ -189,10 +189,10 @@ public void resetRotation(Rotation3d rotation) {
 
     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)
-      );
+      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 {
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index b472868a182..a55d1197e13 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -140,16 +140,18 @@ class WPILIB_DLLEXPORT PoseEstimator {
     m_odometry.ResetTranslation(translation);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-      m_visionUpdates.empty() ? std::nullopt : std::optional{*m_visionUpdates.crbegin()};
+        m_visionUpdates.crbegin() != m_visionUpdates.crend()
+            ? std::optional{*m_visionUpdates.crbegin()}
+            : std::nullopt;
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose rotation
       VisionUpdate const visionUpdate{
-        Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
-        Pose2d{translation, latestVisionUpdate->second.odometryPose.Rotation()}
-      };
+          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 {
@@ -166,16 +168,18 @@ class WPILIB_DLLEXPORT PoseEstimator {
     m_odometry.ResetRotation(rotation);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-      m_visionUpdates.empty() ? std::nullopt : std::optional{*m_visionUpdates.crbegin()};
+        m_visionUpdates.crbegin() != m_visionUpdates.crend()
+            ? std::optional{*m_visionUpdates.crbegin()}
+            : std::nullopt;
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose translation
       VisionUpdate const visionUpdate{
-        Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
-        Pose2d{latestVisionUpdate->second.odometryPose.Translation(), rotation}
-      };
+          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 {
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
index c6f43fef651..7b30d113bcc 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -149,16 +149,18 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     m_odometry.ResetTranslation(translation);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-      m_visionUpdates.empty() ? std::nullopt : std::optional{*m_visionUpdates.crbegin()};
+        m_visionUpdates.crbegin() != m_visionUpdates.crend()
+            ? std::optional{*m_visionUpdates.crbegin()}
+            : std::nullopt;
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose rotation
       VisionUpdate const visionUpdate{
-        Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
-        Pose3d{translation, latestVisionUpdate->second.odometryPose.Rotation()}
-      };
+          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 {
@@ -175,16 +177,18 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     m_odometry.ResetRotation(rotation);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-      m_visionUpdates.empty() ? std::nullopt : std::optional{*m_visionUpdates.crbegin()};
+        m_visionUpdates.crbegin() != m_visionUpdates.crend()
+            ? std::optional{*m_visionUpdates.crbegin()}
+            : std::nullopt;
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose translation
       VisionUpdate const visionUpdate{
-        Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
-        Pose3d{latestVisionUpdate->second.odometryPose.Translation(), rotation}
-      };
+          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 {

From 9ecbc1881339d84da9ea2e04a630057febe8a328 Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Fri, 10 Oct 2025 00:52:49 -0400
Subject: [PATCH 4/8] Silence false positive maybe-unused warning

---
 .../include/frc/estimator/PoseEstimator.h     | 24 ++++++++++++++-----
 .../include/frc/estimator/PoseEstimator3d.h   | 24 ++++++++++++++-----
 2 files changed, 36 insertions(+), 12 deletions(-)

diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index a55d1197e13..e5a8163eaa6 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -136,13 +136,16 @@ 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);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.crbegin() != m_visionUpdates.crend()
-            ? std::optional{*m_visionUpdates.crbegin()}
-            : std::nullopt;
+        m_visionUpdates.empty() ? std::nullopt
+                                : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
@@ -158,19 +161,25 @@ class WPILIB_DLLEXPORT PoseEstimator {
       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);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.crbegin() != m_visionUpdates.crend()
-            ? std::optional{*m_visionUpdates.crbegin()}
-            : std::nullopt;
+        m_visionUpdates.empty() ? std::nullopt
+                                : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
@@ -186,6 +195,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
       m_poseEstimate = m_odometry.GetPose();
     }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Gets the estimated robot pose.
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
index 7b30d113bcc..3c06701076f 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -145,13 +145,16 @@ 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);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.crbegin() != m_visionUpdates.crend()
-            ? std::optional{*m_visionUpdates.crbegin()}
-            : std::nullopt;
+        m_visionUpdates.empty() ? std::nullopt
+                                : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
@@ -167,19 +170,25 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
       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);
 
     std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.crbegin() != m_visionUpdates.crend()
-            ? std::optional{*m_visionUpdates.crbegin()}
-            : std::nullopt;
+        m_visionUpdates.empty() ? std::nullopt
+                                : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
 
@@ -195,6 +204,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
       m_poseEstimate = m_odometry.GetPose();
     }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Gets the estimated robot pose.

From e680f8112d1ff6336fad6fa4ebbb6e1dd6f7e288 Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Sun, 2 Nov 2025 14:48:39 -0500
Subject: [PATCH 5/8] Update comment on m_visionUpdates

---
 .../main/java/edu/wpi/first/math/estimator/PoseEstimator.java | 4 +++-
 .../java/edu/wpi/first/math/estimator/PoseEstimator3d.java    | 4 +++-
 wpimath/src/main/native/include/frc/estimator/PoseEstimator.h | 4 +++-
 .../src/main/native/include/frc/estimator/PoseEstimator3d.h   | 4 +++-
 4 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
index 49cf94a5f8c..2b6d0370232 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -48,7 +48,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;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
index 4d7ede2b239..b3d114f7e0b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
@@ -56,7 +56,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;
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index e5a8163eaa6..d9fdcc5a1bf 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -475,7 +475,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;
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
index 3c06701076f..6c270064437 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -486,7 +486,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;

From 8f57b2a2a704b205cdc37873046daf93e5a53ad8 Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Sun, 2 Nov 2025 23:13:21 -0500
Subject: [PATCH 6/8] Fix usage of const to be on the left

---
 .../include/frc/estimator/PoseEstimator.h      | 18 ++++++++++--------
 .../include/frc/estimator/PoseEstimator3d.h    | 18 ++++++++++--------
 2 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index d9fdcc5a1bf..65681f0c659 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -143,15 +143,16 @@ class WPILIB_DLLEXPORT PoseEstimator {
   void ResetTranslation(const Translation2d& translation) {
     m_odometry.ResetTranslation(translation);
 
-    std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.empty() ? std::nullopt
-                                : std::optional{*m_visionUpdates.crbegin()};
+    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();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose rotation
-      VisionUpdate const visionUpdate{
+      const VisionUpdate visionUpdate{
           Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
           Pose2d{translation,
                  latestVisionUpdate->second.odometryPose.Rotation()}};
@@ -177,15 +178,16 @@ class WPILIB_DLLEXPORT PoseEstimator {
   void ResetRotation(const Rotation2d& rotation) {
     m_odometry.ResetRotation(rotation);
 
-    std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.empty() ? std::nullopt
-                                : std::optional{*m_visionUpdates.crbegin()};
+    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();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose translation
-      VisionUpdate const visionUpdate{
+      const VisionUpdate visionUpdate{
           Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
           Pose2d{latestVisionUpdate->second.odometryPose.Translation(),
                  rotation}};
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
index 6c270064437..f93eb82e342 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
@@ -152,15 +152,16 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
   void ResetTranslation(const Translation3d& translation) {
     m_odometry.ResetTranslation(translation);
 
-    std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.empty() ? std::nullopt
-                                : std::optional{*m_visionUpdates.crbegin()};
+    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();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose rotation
-      VisionUpdate const visionUpdate{
+      const VisionUpdate visionUpdate{
           Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
           Pose3d{translation,
                  latestVisionUpdate->second.odometryPose.Rotation()}};
@@ -186,15 +187,16 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
   void ResetRotation(const Rotation3d& rotation) {
     m_odometry.ResetRotation(rotation);
 
-    std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
-        m_visionUpdates.empty() ? std::nullopt
-                                : std::optional{*m_visionUpdates.crbegin()};
+    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();
 
     if (latestVisionUpdate) {
       // apply vision compensation to the pose translation
-      VisionUpdate const visionUpdate{
+      const VisionUpdate visionUpdate{
           Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
           Pose3d{latestVisionUpdate->second.odometryPose.Translation(),
                  rotation}};

From d33cb7ad418172ca25f55cd16c648ef3e7ead31f Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Sat, 22 Nov 2025 21:39:58 -0500
Subject: [PATCH 7/8] Update SwerveDrivePoseEstimator unit tests to test with
 vision

---
 .../SwerveDrivePoseEstimator3dTest.java       | 33 +++++++++++++++++--
 .../SwerveDrivePoseEstimatorTest.java         | 28 ++++++++++++++--
 .../SwerveDrivePoseEstimator3dTest.cpp        | 29 ++++++++++++++--
 .../SwerveDrivePoseEstimatorTest.cpp          | 25 ++++++++++++--
 4 files changed, 103 insertions(+), 12 deletions(-)

diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
index e3cd11a2815..b49bc43941b 100644
--- 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.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 @@ void testReset() {
         () -> 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 @@ void testReset() {
     }
 
     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,20 @@ void testReset() {
             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.k180deg)),
+        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 +580,7 @@ void testReset() {
         () -> 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);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index 2b259a6a894..f0da1d6feea 100644
--- 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.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,20 @@ void testReset() {
         () ->
             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 +514,7 @@ void testReset() {
     }
 
     assertAll(
-        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
         () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
         () ->
             assertEquals(
@@ -512,6 +522,18 @@ void testReset() {
                 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 +542,7 @@ void testReset() {
         () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
         () ->
             assertEquals(
-                Math.PI / 2,
+                Math.PI * 3.0 / 4,
                 estimator.getEstimatedPosition().getRotation().getRadians(),
                 kEpsilon));
 
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
index 612862f4f15..31025fc95fb 100644
--- 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,18 @@ 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 +510,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
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index 1655074209b..369c3402cbd 100644
--- 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,29 @@ 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

From f33906b740d9a8e935d3d3cca908cac7347f763d Mon Sep 17 00:00:00 2001
From: Benjamin Hall <bhall@ctr-electronics.com>
Date: Sat, 22 Nov 2025 21:48:36 -0500
Subject: [PATCH 8/8] Fix formatting

---
 .../SwerveDrivePoseEstimator3dTest.java          | 16 ++++++++++------
 .../estimator/SwerveDrivePoseEstimatorTest.java  |  6 ++++--
 .../estimator/SwerveDrivePoseEstimator3dTest.cpp |  5 +++--
 .../estimator/SwerveDrivePoseEstimatorTest.cpp   |  5 +++--
 4 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
index b49bc43941b..06cc16e52bc 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
@@ -511,8 +511,8 @@ void testReset() {
         () -> 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());
+    estimator.addVisionMeasurement(
+        new Pose3d(3, 0, 0, Rotation3d.kZero), MathSharedStore.getTimestamp());
 
     assertAll(
         () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
@@ -556,8 +556,8 @@ void testReset() {
                 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.k180deg)),
-        MathSharedStore.getTimestamp());
+    estimator.addVisionMeasurement(
+        new Pose3d(2.5, 1, 0, new Rotation3d(Rotation2d.kPi)), MathSharedStore.getTimestamp());
 
     assertAll(
         () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
@@ -567,7 +567,9 @@ void testReset() {
         () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
         () ->
             assertEquals(
-                Math.PI * 3.0 / 4, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                kEpsilon));
 
     // Test reset translation
     estimator.resetTranslation(new Translation3d(-1, -1, -1));
@@ -580,7 +582,9 @@ void testReset() {
         () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
         () ->
             assertEquals(
-                Math.PI * 3.0 / 4, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
+                Math.PI * 3.0 / 4,
+                estimator.getEstimatedPosition().getRotation().getZ(),
+                kEpsilon));
 
     // Test reset pose
     estimator.resetPose(Pose3d.kZero);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index f0da1d6feea..c2b6c657437 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -483,7 +483,8 @@ void testReset() {
             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());
+    estimator.addVisionMeasurement(
+        new Pose2d(3, 0, Rotation2d.kZero), MathSharedStore.getTimestamp());
 
     assertAll(
         () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
@@ -523,7 +524,8 @@ void testReset() {
                 kEpsilon));
 
     // Add a vision measurement with a different rotation
-    estimator.addVisionMeasurement(new Pose2d(2.5, 1, Rotation2d.kPi), MathSharedStore.getTimestamp());
+    estimator.addVisionMeasurement(
+        new Pose2d(2.5, 1, Rotation2d.kPi), MathSharedStore.getTimestamp());
 
     assertAll(
         () -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
index 31025fc95fb..fa405ae0b6f 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
@@ -455,7 +455,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
 
   // Add a vision measurement with a different translation
   estimator.AddVisionMeasurement(frc::Pose3d(3_m, 0_m, 0_m, frc::Rotation3d{}),
-      wpi::math::MathSharedStore::GetTimestamp());
+                                 wpi::math::MathSharedStore::GetTimestamp());
 
   EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -491,7 +491,8 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
                    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}}),
+  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());
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index 369c3402cbd..5bc75b39bd0 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -432,7 +432,7 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
 
   // Add a vision measurement with a different translation
   estimator.AddVisionMeasurement(frc::Pose2d{3_m, 0_m, frc::Rotation2d{}},
-      wpi::math::MathSharedStore::GetTimestamp());
+                                 wpi::math::MathSharedStore::GetTimestamp());
 
   EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
   EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
@@ -462,7 +462,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
       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}},
+  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());