[wpimath] Fix pose estimator reset methods (#7225)

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

@@ -134,6 +134,8 @@ public class PoseEstimator<T> {
   public void resetPose(Pose2d pose) {
     m_odometry.resetPose(pose);
     m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPoseMeters();
   }
 
   /**
@@ -144,6 +146,8 @@ public class PoseEstimator<T> {
   public void resetTranslation(Translation2d translation) {
     m_odometry.resetTranslation(translation);
     m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPoseMeters();
   }
 
   /**
@@ -154,6 +158,8 @@ public class PoseEstimator<T> {
   public void resetRotation(Rotation2d rotation) {
     m_odometry.resetRotation(rotation);
     m_odometryPoseBuffer.clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPoseMeters();
   }
 
   /**

wpimath/src/main/native/include/frc/estimator/PoseEstimator.h

@@ -124,6 +124,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
   void ResetPose(const Pose2d& pose) {
     m_odometry.ResetPose(pose);
     m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
   }
 
   /**
@@ -134,6 +136,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
   void ResetTranslation(const Translation2d& translation) {
     m_odometry.ResetTranslation(translation);
     m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
   }
 
   /**
@@ -144,6 +148,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
   void ResetRotation(const Rotation2d& rotation) {
     m_odometry.ResetRotation(rotation);
     m_odometryPoseBuffer.Clear();
+    m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.GetPose();
   }
 
   /**

wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
@@ -25,6 +26,8 @@ import java.util.function.Function;
 import org.junit.jupiter.api.Test;
 
 class DifferentialDrivePoseEstimatorTest {
+  private static final double kEpsilon = 1e-9;
+
   @Test
   void testAccuracy() {
     var kinematics = new DifferentialDriveKinematics(1);
@@ -355,4 +358,81 @@ class DifferentialDrivePoseEstimatorTest {
     assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
     assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
   }
+
+  @Test
+  void testReset() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            0,
+            0,
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Test reset position
+    estimator.resetPosition(Rotation2d.kZero, 1, 1, new Pose2d(1, 0, Rotation2d.kZero));
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test orientation and wheel positions
+    estimator.update(Rotation2d.kZero, 2, 2);
+
+    assertAll(
+        () -> assertEquals(2, 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(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test orientation
+    estimator.update(Rotation2d.kZero, 3, 3);
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation2d(-1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose2d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
@@ -26,6 +27,8 @@ import java.util.function.Function;
 import org.junit.jupiter.api.Test;
 
 class MecanumDrivePoseEstimatorTest {
+  private static final double kEpsilon = 1e-9;
+
   @Test
   void testAccuracyFacingTrajectory() {
     var kinematics =
@@ -377,4 +380,88 @@ class MecanumDrivePoseEstimatorTest {
     assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
     assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
   }
+
+  @Test
+  void testReset() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new MecanumDriveWheelPositions(),
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Test reset position
+    estimator.resetPosition(
+        Rotation2d.kZero,
+        new MecanumDriveWheelPositions(1, 1, 1, 1),
+        new Pose2d(1, 0, Rotation2d.kZero));
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test orientation and wheel positions
+    estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(2, 2, 2, 2));
+
+    assertAll(
+        () -> assertEquals(2, 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(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test orientation
+    estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(3, 3, 3, 3));
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation2d(-1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose2d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
@@ -26,6 +27,8 @@ import java.util.function.Function;
 import org.junit.jupiter.api.Test;
 
 class SwerveDrivePoseEstimatorTest {
+  private static final double kEpsilon = 1e-9;
+
   @Test
   void testAccuracyFacingTrajectory() {
     var kinematics =
@@ -422,4 +425,112 @@ class SwerveDrivePoseEstimatorTest {
     assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
     assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
   }
+
+  @Test
+  void testReset() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            Rotation2d.kZero,
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            Pose2d.kZero,
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Test reset position
+    {
+      var modulePosition = new SwerveModulePosition(1, Rotation2d.kZero);
+      estimator.resetPosition(
+          Rotation2d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          },
+          new Pose2d(1, 0, Rotation2d.kZero));
+    }
+
+    assertAll(
+        () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+
+    // Test orientation and wheel positions
+    {
+      var modulePosition = new SwerveModulePosition(2, Rotation2d.kZero);
+      estimator.update(
+          Rotation2d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          });
+    }
+
+    assertAll(
+        () -> assertEquals(2, 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(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test orientation
+    {
+      var modulePosition = new SwerveModulePosition(3, Rotation2d.kZero);
+      estimator.update(
+          Rotation2d.kZero,
+          new SwerveModulePosition[] {
+            modulePosition, modulePosition, modulePosition, modulePosition
+          });
+    }
+
+    assertAll(
+        () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset translation
+    estimator.resetTranslation(new Translation2d(-1, -1));
+
+    assertAll(
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                Math.PI / 2,
+                estimator.getEstimatedPosition().getRotation().getRadians(),
+                kEpsilon));
+
+    // Test reset pose
+    estimator.resetPose(Pose2d.kZero);
+
+    assertAll(
+        () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
+        () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
+        () ->
+            assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
+  }
 }

wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <limits>
+#include <numbers>
 #include <random>
 #include <tuple>
 #include <utility>
@@ -354,3 +355,56 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
   EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
             estimator.SampleAt(2.5_s));
 }
+
+TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
+  frc::DifferentialDriveKinematics kinematics{1_m};
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,      frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+      {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
+
+  // Test reset position
+  estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m,
+                          frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
+
+  EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test orientation and wheel positions
+  estimator.Update(frc::Rotation2d{}, 2_m, 2_m);
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset rotation
+  estimator.ResetRotation(frc::Rotation2d{90_deg});
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test orientation
+  estimator.Update(frc::Rotation2d{}, 3_m, 3_m);
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset translation
+  estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
+
+  EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset pose
+  estimator.ResetPose(frc::Pose2d{});
+
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+}

wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <limits>
+#include <numbers>
 #include <random>
 #include <tuple>
 #include <vector>
@@ -361,3 +362,58 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
   EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
             estimator.SampleAt(2.5_s));
 }
+
+TEST(MecanumDrivePoseEstimatorTest, TestReset) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+  frc::MecanumDrivePoseEstimator estimator{
+      kinematics,    frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
+      frc::Pose2d{}, {1.0, 1.0, 1.0},   {1.0, 1.0, 1.0}};
+
+  // Test reset position
+  estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m},
+                          frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
+
+  EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test orientation and wheel positions
+  estimator.Update(frc::Rotation2d{}, {2_m, 2_m, 2_m, 2_m});
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset rotation
+  estimator.ResetRotation(frc::Rotation2d{90_deg});
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test orientation
+  estimator.Update(frc::Rotation2d{}, {3_m, 3_m, 3_m, 3_m});
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset translation
+  estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
+
+  EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset pose
+  estimator.ResetPose(frc::Pose2d{});
+
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+}

wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <limits>
+#include <numbers>
 #include <random>
 #include <tuple>
 #include <vector>
@@ -383,3 +384,76 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
   EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
             estimator.SampleAt(2.5_s));
 }
+
+TEST(SwerveDrivePoseEstimatorTest, TestReset) {
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+  frc::SwerveDrivePoseEstimator estimator{
+      kinematics,
+      frc::Rotation2d{},
+      {frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
+       frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
+      frc::Pose2d{},
+      {1.0, 1.0, 1.0},
+      {1.0, 1.0, 1.0}};
+
+  // Test reset position
+  {
+    frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};
+    estimator.ResetPosition(
+        frc::Rotation2d{},
+        {modulePosition, modulePosition, modulePosition, modulePosition},
+        frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
+  }
+
+  EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test orientation and wheel positions
+  {
+    frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}};
+    estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
+                                         modulePosition, modulePosition});
+  }
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset rotation
+  estimator.ResetRotation(frc::Rotation2d{90_deg});
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test orientation
+  {
+    frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}};
+    estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
+                                         modulePosition, modulePosition});
+  }
+
+  EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset translation
+  estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
+
+  EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(std::numbers::pi / 2,
+            estimator.GetEstimatedPosition().Rotation().Radians().value());
+
+  // Test reset pose
+  estimator.ResetPose(frc::Pose2d{});
+
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
+  EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
+}