[wpimath] Fix pose estimator performance (#4111)

Fixes #4087.

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.