[wpimath] Fix pose estimator performance (#4111)

Fixes #4087.

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

@@ -11,6 +11,7 @@ import edu.wpi.first.math.StateSpaceUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
@@ -55,7 +56,7 @@ import java.util.function.BiConsumer;
 public class DifferentialDrivePoseEstimator {
   final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing
   private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
-  private final KalmanFilterLatencyCompensator<N5, N3, N3> m_latencyCompensator;
+  private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
 
   private final double m_nominalDt; // Seconds
   private double m_prevTimeSeconds = -1.0;
@@ -135,7 +136,7 @@ public class DifferentialDrivePoseEstimator {
             AngleStatistics.angleResidual(2),
             AngleStatistics.angleAdd(2),
             m_nominalDt);
-    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+    m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
 
     // Initialize vision R
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
@@ -221,7 +222,7 @@ public class DifferentialDrivePoseEstimator {
   public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
     // Reset state estimate and error covariance
     m_observer.reset();
-    m_latencyCompensator.reset();
+    m_poseBuffer.clear();
 
     m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
 
@@ -246,6 +247,10 @@ public class DifferentialDrivePoseEstimator {
    * <p>This method can be called as infrequently as you want, as long as you are calling {@link
    * DifferentialDrivePoseEstimator#update} every loop.
    *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
    * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
    * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
    *     don't use your own time source by calling {@link
@@ -255,13 +260,13 @@ public class DifferentialDrivePoseEstimator {
    *     source in this case.
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    m_latencyCompensator.applyPastGlobalMeasurement(
-        Nat.N3(),
-        m_observer,
-        m_nominalDt,
-        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
-        m_visionCorrect,
-        timestampSeconds);
+    var sample = m_poseBuffer.getSample(timestampSeconds);
+    if (sample.isPresent()) {
+      m_visionCorrect.accept(
+          new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0),
+          StateSpaceUtil.poseTo3dVector(
+              getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
+    }
   }
 
   /**
@@ -271,6 +276,10 @@ public class DifferentialDrivePoseEstimator {
    * <p>This method can be called as infrequently as you want, as long as you are calling {@link
    * DifferentialDrivePoseEstimator#update} every loop.
    *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
    * <p>Note that the vision measurement standard deviations passed into this method will continue
    * to apply to future measurements until a subsequent call to {@link
    * DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
@@ -354,7 +363,7 @@ public class DifferentialDrivePoseEstimator {
     m_previousAngle = angle;
 
     var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
     m_observer.predict(u, dt);
     m_observer.correct(u, localY);
 

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

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -11,6 +12,7 @@ import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
 import edu.wpi.first.math.numbers.N1;
@@ -48,7 +50,7 @@ public class MecanumDrivePoseEstimator {
   private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
   private final MecanumDriveKinematics m_kinematics;
   private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
-  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+  private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
 
   private final double m_nominalDt; // Seconds
   private double m_prevTimeSeconds = -1.0;
@@ -134,7 +136,7 @@ public class MecanumDrivePoseEstimator {
             AngleStatistics.angleAdd(2),
             m_nominalDt);
     m_kinematics = kinematics;
-    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+    m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
 
     // Initialize vision R
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
@@ -184,7 +186,7 @@ public class MecanumDrivePoseEstimator {
   public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
     // Reset state estimate and error covariance
     m_observer.reset();
-    m_latencyCompensator.reset();
+    m_poseBuffer.clear();
 
     m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
 
@@ -209,6 +211,10 @@ public class MecanumDrivePoseEstimator {
    * <p>This method can be called as infrequently as you want, as long as you are calling {@link
    * MecanumDrivePoseEstimator#update} every loop.
    *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
    * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
    * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
    *     don't use your own time source by calling {@link MecanumDrivePoseEstimator#updateWithTime}
@@ -217,13 +223,13 @@ public class MecanumDrivePoseEstimator {
    *     Timer.getFPGATimestamp as your time source or sync the epochs.
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    m_latencyCompensator.applyPastGlobalMeasurement(
-        Nat.N3(),
-        m_observer,
-        m_nominalDt,
-        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
-        m_visionCorrect,
-        timestampSeconds);
+    var sample = m_poseBuffer.getSample(timestampSeconds);
+    if (sample.isPresent()) {
+      m_visionCorrect.accept(
+          new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0),
+          StateSpaceUtil.poseTo3dVector(
+              getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
+    }
   }
 
   /**
@@ -233,6 +239,10 @@ public class MecanumDrivePoseEstimator {
    * <p>This method can be called as infrequently as you want, as long as you are calling {@link
    * MecanumDrivePoseEstimator#update} every loop.
    *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
    * <p>Note that the vision measurement standard deviations passed into this method will continue
    * to apply to future measurements until a subsequent call to {@link
    * MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
@@ -296,7 +306,7 @@ public class MecanumDrivePoseEstimator {
     m_previousAngle = angle;
 
     var localY = VecBuilder.fill(angle.getRadians());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
     m_observer.predict(u, dt);
     m_observer.correct(u, localY);
 

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

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
+import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -11,6 +12,7 @@ import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N1;
@@ -48,7 +50,7 @@ public class SwerveDrivePoseEstimator {
   private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
   private final SwerveDriveKinematics m_kinematics;
   private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
-  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+  private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
 
   private final double m_nominalDt; // Seconds
   private double m_prevTimeSeconds = -1.0;
@@ -134,7 +136,7 @@ public class SwerveDrivePoseEstimator {
             AngleStatistics.angleAdd(2),
             m_nominalDt);
     m_kinematics = kinematics;
-    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+    m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
 
     // Initialize vision R
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
@@ -182,7 +184,7 @@ public class SwerveDrivePoseEstimator {
   public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
     // Reset state estimate and error covariance
     m_observer.reset();
-    m_latencyCompensator.reset();
+    m_poseBuffer.clear();
 
     m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
 
@@ -207,6 +209,10 @@ public class SwerveDrivePoseEstimator {
    * <p>This method can be called as infrequently as you want, as long as you are calling {@link
    * SwerveDrivePoseEstimator#update} every loop.
    *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
    * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
    * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
    *     don't use your own time source by calling {@link SwerveDrivePoseEstimator#updateWithTime}
@@ -215,13 +221,13 @@ public class SwerveDrivePoseEstimator {
    *     Timer.getFPGATimestamp as your time source or sync the epochs.
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    m_latencyCompensator.applyPastGlobalMeasurement(
-        Nat.N3(),
-        m_observer,
-        m_nominalDt,
-        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
-        m_visionCorrect,
-        timestampSeconds);
+    var sample = m_poseBuffer.getSample(timestampSeconds);
+    if (sample.isPresent()) {
+      m_visionCorrect.accept(
+          new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0),
+          StateSpaceUtil.poseTo3dVector(
+              getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
+    }
   }
 
   /**
@@ -231,6 +237,10 @@ public class SwerveDrivePoseEstimator {
    * <p>This method can be called as infrequently as you want, as long as you are calling {@link
    * SwerveDrivePoseEstimator#update} every loop.
    *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
    * <p>Note that the vision measurement standard deviations passed into this method will continue
    * to apply to future measurements until a subsequent call to {@link
    * SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
@@ -294,7 +304,7 @@ public class SwerveDrivePoseEstimator {
     m_previousAngle = angle;
 
     var localY = VecBuilder.fill(angle.getRadians());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
     m_observer.predict(u, dt);
     m_observer.correct(u, localY);
 

wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java

@@ -6,6 +6,7 @@ package edu.wpi.first.math.interpolation;
 
 import edu.wpi.first.math.MathUtil;
 import java.util.NavigableMap;
+import java.util.Optional;
 import java.util.TreeMap;
 
 /**
@@ -19,7 +20,7 @@ import java.util.TreeMap;
 public final class TimeInterpolatableBuffer<T> {
   private final double m_historySize;
   private final InterpolateFunction<T> m_interpolatingFunc;
-  private final NavigableMap<Double, T> m_buffer = new TreeMap<>();
+  private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
 
   private TimeInterpolatableBuffer(
       InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
@@ -70,7 +71,7 @@ public final class TimeInterpolatableBuffer<T> {
    */
   public void addSample(double timeSeconds, T sample) {
     cleanUp(timeSeconds);
-    m_buffer.put(timeSeconds, sample);
+    m_pastSnapshots.put(timeSeconds, sample);
   }
 
   /**
@@ -79,10 +80,10 @@ public final class TimeInterpolatableBuffer<T> {
    * @param time The current timestamp.
    */
   private void cleanUp(double time) {
-    while (!m_buffer.isEmpty()) {
-      var entry = m_buffer.firstEntry();
+    while (!m_pastSnapshots.isEmpty()) {
+      var entry = m_pastSnapshots.firstEntry();
       if (time - entry.getKey() >= m_historySize) {
-        m_buffer.remove(entry.getKey());
+        m_pastSnapshots.remove(entry.getKey());
       } else {
         return;
       }
@@ -91,45 +92,46 @@ public final class TimeInterpolatableBuffer<T> {
 
   /** Clear all old samples. */
   public void clear() {
-    m_buffer.clear();
+    m_pastSnapshots.clear();
   }
 
   /**
-   * Sample the buffer at the given time. If the buffer is empty, this will return null.
+   * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
    *
    * @param timeSeconds The time at which to sample.
-   * @return The interpolated value at that timestamp. Might be null.
+   * @return The interpolated value at that timestamp or an empty Optional.
    */
   @SuppressWarnings("UnnecessaryParentheses")
-  public T getSample(double timeSeconds) {
-    if (m_buffer.isEmpty()) {
-      return null;
+  public Optional<T> getSample(double timeSeconds) {
+    if (m_pastSnapshots.isEmpty()) {
+      return Optional.empty();
     }
 
     // Special case for when the requested time is the same as a sample
-    var nowEntry = m_buffer.get(timeSeconds);
+    var nowEntry = m_pastSnapshots.get(timeSeconds);
     if (nowEntry != null) {
-      return nowEntry;
+      return Optional.of(nowEntry);
     }
 
-    var topBound = m_buffer.ceilingEntry(timeSeconds);
-    var bottomBound = m_buffer.floorEntry(timeSeconds);
+    var topBound = m_pastSnapshots.ceilingEntry(timeSeconds);
+    var bottomBound = m_pastSnapshots.floorEntry(timeSeconds);
 
     // Return null if neither sample exists, and the opposite bound if the other is null
     if (topBound == null && bottomBound == null) {
-      return null;
+      return Optional.empty();
     } else if (topBound == null) {
-      return bottomBound.getValue();
+      return Optional.of(bottomBound.getValue());
     } else if (bottomBound == null) {
-      return topBound.getValue();
+      return Optional.of(topBound.getValue());
     } else {
       // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
       // between the current time and bottom bound) and (the difference between top and bottom
       // bounds).
-      return m_interpolatingFunc.interpolate(
-          bottomBound.getValue(),
-          topBound.getValue(),
-          ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+      return Optional.of(
+          m_interpolatingFunc.interpolate(
+              bottomBound.getValue(),
+              topBound.getValue(),
+              ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey()))));
     }
   }
 

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -56,7 +56,7 @@ void DifferentialDrivePoseEstimator::ResetPosition(
     const Pose2d& pose, const Rotation2d& gyroAngle) {
   // Reset state estimate and error covariance
   m_observer.Reset();
-  m_latencyCompensator.Reset();
+  m_poseBuffer.Clear();
 
   m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
 
@@ -72,9 +72,11 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
 
 void DifferentialDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
-  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
-      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
-      m_visionCorrect, timestamp);
+  if (auto sample = m_poseBuffer.Sample(timestamp)) {
+    m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
+                    PoseTo3dVector(GetEstimatedPosition().TransformBy(
+                        visionRobotPose - sample.value())));
+  }
 }
 
 Pose2d DifferentialDrivePoseEstimator::Update(
@@ -103,7 +105,7 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
   auto localY = Eigen::Vector<double, 3>{
       leftDistance.value(), rightDistance.value(), angle.Radians().value()};
 
-  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+  m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
   m_observer.Predict(u, dt);
   m_observer.Correct(u, localY);
 

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -60,7 +60,7 @@ void frc::MecanumDrivePoseEstimator::ResetPosition(
     const Pose2d& pose, const Rotation2d& gyroAngle) {
   // Reset state estimate and error covariance
   m_observer.Reset();
-  m_latencyCompensator.Reset();
+  m_poseBuffer.Clear();
 
   m_observer.SetXhat(PoseTo3dVector(pose));
 
@@ -75,9 +75,11 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
 
 void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
-  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
-      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
-      m_visionCorrect, timestamp);
+  if (auto sample = m_poseBuffer.Sample(timestamp)) {
+    m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
+                    PoseTo3dVector(GetEstimatedPosition().TransformBy(
+                        visionRobotPose - sample.value())));
+  }
 }
 
 Pose2d frc::MecanumDrivePoseEstimator::Update(
@@ -107,7 +109,7 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
   Eigen::Vector<double, 1> localY{angle.Radians().value()};
   m_previousAngle = angle;
 
-  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+  m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
 
   m_observer.Predict(u, dt);
   m_observer.Correct(u, localY);

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

@@ -8,10 +8,10 @@
 #include <wpi/array.h>
 
 #include "Eigen/Core"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
 #include "units/time.h"
 
@@ -127,6 +127,10 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
    * This method can be called as infrequently as you want, as long as you are
    * calling Update() every loop.
    *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
    * @param visionRobotPose The pose of the robot as measured by the vision
    *                        camera.
    * @param timestamp       The timestamp of the vision measurement in seconds.
@@ -148,6 +152,10 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
    * This method can be called as infrequently as you want, as long as you are
    * calling Update() every loop.
    *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
    * Note that the vision measurement standard deviations passed into this
    * method will continue to apply to future measurements until a subsequent
    * call to SetVisionMeasurementStdDevs() or this method.
@@ -214,8 +222,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
 
  private:
   UnscentedKalmanFilter<5, 3, 3> m_observer;
-  KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>>
-      m_latencyCompensator;
+  TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
   std::function<void(const Eigen::Vector<double, 3>& u,
                      const Eigen::Vector<double, 3>& y)>
       m_visionCorrect;

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

@@ -10,10 +10,10 @@
 #include <wpi/array.h>
 
 #include "Eigen/Core"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
 #include "units/time.h"
 
@@ -123,6 +123,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
    * This method can be called as infrequently as you want, as long as you are
    * calling Update() every loop.
    *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
    * @param visionRobotPose The pose of the robot as measured by the vision
    *                        camera.
    * @param timestamp       The timestamp of the vision measurement in seconds.
@@ -144,6 +148,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
    * This method can be called as infrequently as you want, as long as you are
    * calling Update() every loop.
    *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
    * Note that the vision measurement standard deviations passed into this
    * method will continue to apply to future measurements until a subsequent
    * call to SetVisionMeasurementStdDevs() or this method.
@@ -204,8 +212,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
  private:
   UnscentedKalmanFilter<3, 3, 1> m_observer;
   MecanumDriveKinematics m_kinematics;
-  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
-      m_latencyCompensator;
+  TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
   std::function<void(const Eigen::Vector<double, 3>& u,
                      const Eigen::Vector<double, 3>& y)>
       m_visionCorrect;

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

@@ -12,10 +12,10 @@
 #include "Eigen/Core"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "units/time.h"
 
@@ -127,7 +127,7 @@ class SwerveDrivePoseEstimator {
   void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
     // Reset state estimate and error covariance
     m_observer.Reset();
-    m_latencyCompensator.Reset();
+    m_poseBuffer.Clear();
 
     m_observer.SetXhat(PoseTo3dVector(pose));
 
@@ -171,6 +171,10 @@ class SwerveDrivePoseEstimator {
    * This method can be called as infrequently as you want, as long as you are
    * calling Update() every loop.
    *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
    * @param visionRobotPose The pose of the robot as measured by the vision
    *                        camera.
    * @param timestamp       The timestamp of the vision measurement in seconds.
@@ -184,9 +188,11 @@ class SwerveDrivePoseEstimator {
    */
   void AddVisionMeasurement(const Pose2d& visionRobotPose,
                             units::second_t timestamp) {
-    m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
-        &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
-        m_visionCorrect, timestamp);
+    if (auto sample = m_poseBuffer.Sample(timestamp)) {
+      m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
+                      PoseTo3dVector(GetEstimatedPosition().TransformBy(
+                          visionRobotPose - sample.value())));
+    }
   }
 
   /**
@@ -196,6 +202,10 @@ class SwerveDrivePoseEstimator {
    * This method can be called as infrequently as you want, as long as you are
    * calling Update() every loop.
    *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
    * Note that the vision measurement standard deviations passed into this
    * method will continue to apply to future measurements until a subsequent
    * call to SetVisionMeasurementStdDevs() or this method.
@@ -275,7 +285,7 @@ class SwerveDrivePoseEstimator {
     Eigen::Vector<double, 1> localY{angle.Radians().value()};
     m_previousAngle = angle;
 
-    m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+    m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
 
     m_observer.Predict(u, dt);
     m_observer.Correct(u, localY);
@@ -286,8 +296,7 @@ class SwerveDrivePoseEstimator {
  private:
   UnscentedKalmanFilter<3, 3, 1> m_observer;
   SwerveDriveKinematics<NumModules>& m_kinematics;
-  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
-      m_latencyCompensator;
+  TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
   std::function<void(const Eigen::Vector<double, 3>& u,
                      const Eigen::Vector<double, 3>& y)>
       m_visionCorrect;

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -7,6 +7,7 @@
 #include <array>
 #include <functional>
 #include <map>
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -81,12 +82,16 @@ class TimeInterpolatableBuffer {
   void Clear() { m_pastSnapshots.clear(); }
 
   /**
-   * Sample the buffer at the given time. If there are no elements in the
-   * buffer, calling this function results in undefined behavior.
+   * Sample the buffer at the given time. If the buffer is empty, an empty
+   * optional is returned.
    *
    * @param time The time at which to sample the buffer.
    */
-  T Sample(units::second_t time) {
+  std::optional<T> Sample(units::second_t time) {
+    if (m_pastSnapshots.empty()) {
+      return {};
+    }
+
     // We will perform a binary search to find the index of the element in the
     // vector that has a timestamp that is equal to or greater than the vision
     // measurement timestamp.

wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java

@@ -16,15 +16,15 @@ class TimeInterpolatableBufferTest {
     TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
 
     buffer.addSample(0, new Rotation2d());
-    assertEquals(0, buffer.getSample(0).getRadians(), 0.001);
+    assertEquals(0, buffer.getSample(0).get().getRadians(), 0.001);
     buffer.addSample(1, new Rotation2d(1));
-    assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001);
-    assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001);
+    assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
+    assertEquals(1.0, buffer.getSample(1.0).get().getRadians(), 0.001);
     buffer.addSample(3, new Rotation2d(2));
-    assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001);
+    assertEquals(1.5, buffer.getSample(2).get().getRadians(), 0.001);
 
     buffer.addSample(10.5, new Rotation2d(2));
-    assertEquals(new Rotation2d(1), buffer.getSample(0));
+    assertEquals(new Rotation2d(1), buffer.getSample(0).get());
   }
 
   @Test
@@ -34,7 +34,7 @@ class TimeInterpolatableBufferTest {
     // We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
     buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
     buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
-    Pose2d sample = buffer.getSample(0.5);
+    Pose2d sample = buffer.getSample(0.5).get();
 
     assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
     assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);

wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp

@@ -14,12 +14,12 @@ TEST(TimeInterpolatableBufferTest, Interpolation) {
   frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
 
   buffer.AddSample(0_s, frc::Rotation2d(0_rad));
-  EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad));
+  EXPECT_TRUE(buffer.Sample(0_s).value() == frc::Rotation2d(0_rad));
   buffer.AddSample(1_s, frc::Rotation2d(1_rad));
-  EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad));
-  EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad));
+  EXPECT_TRUE(buffer.Sample(0.5_s).value() == frc::Rotation2d(0.5_rad));
+  EXPECT_TRUE(buffer.Sample(1_s).value() == frc::Rotation2d(1_rad));
   buffer.AddSample(3_s, frc::Rotation2d(2_rad));
-  EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad));
+  EXPECT_TRUE(buffer.Sample(2_s).value() == frc::Rotation2d(1.5_rad));
 
   buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
   EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
@@ -30,9 +30,8 @@ TEST(TimeInterpolatableBufferTest, Pose2d) {
   // We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
   buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
   buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
-  frc::Pose2d sample = buffer.Sample(0.5_s);
-  EXPECT_TRUE(std::abs(sample.X().to<double>() - (1 - 1 / std::sqrt(2))) <
-              0.01);
-  EXPECT_TRUE(std::abs(sample.Y().to<double>() - (1 / std::sqrt(2))) < 0.01);
-  EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to<double>() - 45) < 0.01);
+  frc::Pose2d sample = buffer.Sample(0.5_s).value();
+  EXPECT_TRUE(std::abs(sample.X().value() - (1 - 1 / std::sqrt(2))) < 0.01);
+  EXPECT_TRUE(std::abs(sample.Y().value() - (1 / std::sqrt(2))) < 0.01);
+  EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45) < 0.01);
 }