diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index 9a0aebd799..ea8fc2f6bb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -216,9 +216,14 @@ public class DifferentialDrivePoseEstimator {
    * @param gyroAngle The angle reported by the gyroscope.
    */
   public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
-    m_previousAngle = poseMeters.getRotation();
+    // Reset state estimate and error covariance
+    m_observer.reset();
+    m_latencyCompensator.reset();
+
     m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
+
     m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_previousAngle = poseMeters.getRotation();
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
index 52ebb5c063..bc55cbb714 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
@@ -22,6 +22,11 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
     m_pastObserverSnapshots = new ArrayList<>();
   }
 
+  /** Clears the observer snapshot buffer. */
+  public void reset() {
+    m_pastObserverSnapshots.clear();
+  }
+
   /**
    * Add past observer states to the observer snapshots list.
    *
@@ -51,18 +56,18 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
    * @param rows The rows in the global measurement vector.
    * @param observer The observer to apply the past global measurement.
    * @param nominalDtSeconds The nominal timestep.
-   * @param globalMeasurement The measurement.
+   * @param y The measurement.
    * @param globalMeasurementCorrect The function take calls correct() on the observer.
-   * @param globalMeasurementTimestampSeconds The timestamp of the measurement.
+   * @param timestampSeconds The timestamp of the measurement.
    */
   @SuppressWarnings({"ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
   public <R extends Num> void applyPastGlobalMeasurement(
       Nat<R> rows,
       KalmanTypeFilter<S, I, O> observer,
       double nominalDtSeconds,
-      Matrix<R, N1> globalMeasurement,
+      Matrix<R, N1> y,
       BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
-      double globalMeasurementTimestampSeconds) {
+      double timestampSeconds) {
     if (m_pastObserverSnapshots.isEmpty()) {
       // State map was empty, which means that we got a past measurement right at startup. The only
       // thing we can really do is ignore the measurement.
@@ -77,7 +82,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
 
     while (low != high) {
       int mid = (low + high) / 2;
-      if (m_pastObserverSnapshots.get(mid).getKey() < globalMeasurementTimestampSeconds) {
+      if (m_pastObserverSnapshots.get(mid).getKey() < timestampSeconds) {
         // This index and everything under it are less than the requested timestamp. Therefore, we
         // can discard them.
         low = mid + 1;
@@ -91,7 +96,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
     // We are simply assigning this index to a new variable to avoid confusion
     // with variable names.
     int index = low;
-    double timestamp = globalMeasurementTimestampSeconds;
+    double timestamp = timestampSeconds;
     int indexOfClosestEntry =
         Math.abs(timestamp - m_pastObserverSnapshots.get(index - 1).getKey())
                 <= Math.abs(
@@ -124,7 +129,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
         // timestep for which we called predict.
         // This makes the assumption that the dt is small enough that the difference between the
         // measurement time and the time that the inputs were captured at is very small.
-        globalMeasurementCorrect.accept(snapshot.inputs, globalMeasurement);
+        globalMeasurementCorrect.accept(snapshot.inputs, y);
       }
       lastTimestamp = key;
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index 489d6e7645..78faedae14 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -181,9 +181,14 @@ public class MecanumDrivePoseEstimator {
    * @param gyroAngle The angle reported by the gyroscope.
    */
   public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
-    m_previousAngle = poseMeters.getRotation();
+    // Reset state estimate and error covariance
+    m_observer.reset();
+    m_latencyCompensator.reset();
+
     m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+
     m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_previousAngle = poseMeters.getRotation();
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index b03e90a7db..0b19778a34 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -181,9 +181,14 @@ public class SwerveDrivePoseEstimator {
    * @param gyroAngle The angle reported by the gyroscope.
    */
   public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
-    m_previousAngle = poseMeters.getRotation();
+    // Reset state estimate and error covariance
+    m_observer.reset();
+    m_latencyCompensator.reset();
+
     m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+
     m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_previousAngle = poseMeters.getRotation();
   }
 
   /**
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 3c00154fd4..2d919e6ae0 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -55,9 +55,14 @@ void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
 
 void DifferentialDrivePoseEstimator::ResetPosition(
     const Pose2d& pose, const Rotation2d& gyroAngle) {
-  m_previousAngle = pose.Rotation();
+  // Reset state estimate and error covariance
+  m_observer.Reset();
+  m_latencyCompensator.Reset();
+
   m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
+
   m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
+  m_previousAngle = pose.Rotation();
 }
 
 Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
@@ -68,9 +73,9 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
 
 void DifferentialDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
-  m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
-                                               PoseTo3dVector(visionRobotPose),
-                                               m_visionCorrect, timestamp);
+  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
+      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+      m_visionCorrect, timestamp);
 }
 
 Pose2d DifferentialDrivePoseEstimator::Update(
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 44453b390f..5d6967e3ea 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -60,10 +60,12 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
 
 void frc::MecanumDrivePoseEstimator::ResetPosition(
     const Pose2d& pose, const Rotation2d& gyroAngle) {
-  // Set observer state.
+  // Reset state estimate and error covariance
+  m_observer.Reset();
+  m_latencyCompensator.Reset();
+
   m_observer.SetXhat(PoseTo3dVector(pose));
 
-  // Calculate offsets.
   m_gyroOffset = pose.Rotation() - gyroAngle;
   m_previousAngle = pose.Rotation();
 }
@@ -75,9 +77,9 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
 
 void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
-  m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
-                                               PoseTo3dVector(visionRobotPose),
-                                               m_visionCorrect, timestamp);
+  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
+      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+      m_visionCorrect, timestamp);
 }
 
 Pose2d frc::MecanumDrivePoseEstimator::Update(
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index 57d8a09754..f7e5588c8b 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -34,6 +34,19 @@ class KalmanFilterLatencyCompensator {
           localMeasurements(localY) {}
   };
 
+  /**
+   * Clears the observer snapshot buffer.
+   */
+  void Reset() { m_pastObserverSnapshots.clear(); }
+
+  /**
+   * Add past observer states to the observer snapshots list.
+   *
+   * @param observer  The observer.
+   * @param u         The input at the timestamp.
+   * @param localY    The local output at the timestamp
+   * @param timestamp The timesnap of the state.
+   */
   void AddObserverState(const KalmanFilterType& observer,
                         Eigen::Matrix<double, Inputs, 1> u,
                         Eigen::Matrix<double, Outputs, 1> localY,
@@ -48,8 +61,19 @@ class KalmanFilterLatencyCompensator {
     }
   }
 
+  /**
+   * Add past global measurements (such as from vision)to the estimator.
+   *
+   * @param observer                 The observer to apply the past global
+   *                                 measurement.
+   * @param nominalDt                The nominal timestep.
+   * @param y                        The measurement.
+   * @param globalMeasurementCorrect The function take calls correct() on the
+   *                                 observer.
+   * @param timestamp                The timestamp of the measurement.
+   */
   template <int Rows>
-  void ApplyPastMeasurement(
+  void ApplyPastGlobalMeasurement(
       KalmanFilterType* observer, units::second_t nominalDt,
       Eigen::Matrix<double, Rows, 1> y,
       std::function<void(const Eigen::Matrix<double, Inputs, 1>& u,
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index 950c3fb983..e0f0ae2286 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -125,10 +125,12 @@ class SwerveDrivePoseEstimator {
    * @param gyroAngle The angle reported by the gyroscope.
    */
   void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
-    // Set observer state.
+    // Reset state estimate and error covariance
+    m_observer.Reset();
+    m_latencyCompensator.Reset();
+
     m_observer.SetXhat(PoseTo3dVector(pose));
 
-    // Calculate offsets.
     m_gyroOffset = pose.Rotation() - gyroAngle;
     m_previousAngle = pose.Rotation();
   }
@@ -184,7 +186,7 @@ class SwerveDrivePoseEstimator {
    */
   void AddVisionMeasurement(const Pose2d& visionRobotPose,
                             units::second_t timestamp) {
-    m_latencyCompensator.ApplyPastMeasurement<3>(
+    m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
         &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
         m_visionCorrect, timestamp);
   }