Store latest pose estimate

wpilibsuite · Jun 18, 2024 · 9ea22ee · 9ea22ee
1 parent 251c058
commit 9ea22ee
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 6 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
@@ -50,6 +50,8 @@ public class PoseEstimator<T> {
   // been no vision measurements after the last reset
   private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
 
+  private Pose2d m_poseEstimate;
+
   /**
    * Constructs a PoseEstimator.
    *
@@ -70,6 +72,8 @@ public PoseEstimator(
       Matrix<N3, N1> visionMeasurementStdDevs) {
     m_odometry = odometry;
 
+    m_poseEstimate = m_odometry.getPoseMeters();
+
     for (int i = 0; i < 3; ++i) {
       m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
     }
@@ -118,6 +122,7 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
     m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
+    m_poseEstimate = m_odometry.getPoseMeters();
   }
 
   /**
@@ -126,11 +131,7 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
    * @return The estimated robot pose in meters.
    */
   public Pose2d getEstimatedPosition() {
-    if (m_visionUpdates.isEmpty()) {
-      return m_odometry.getPoseMeters();
-    }
-    var visionUpdate = m_visionUpdates.get(m_visionUpdates.lastKey());
-    return visionUpdate.compensate(m_odometry.getPoseMeters());
+    return m_poseEstimate;
   }
 
   /**
@@ -252,6 +253,10 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS
 
     // Step 8: Remove later vision measurements. (Matches previous behavior)
     m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
+
+    // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
+    // it's guaranteed to be the latest vision update.
+    m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
   }
 
   /**
@@ -314,6 +319,13 @@ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T
 
     m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate);
 
+    if (m_visionUpdates.isEmpty()) {
+      m_poseEstimate = odometryEstimate;
+    } else {
+      var visionUpdate = m_visionUpdates.get(m_visionUpdates.lastKey());
+      m_poseEstimate = visionUpdate.compensate(odometryEstimate);
+    }
+
     return getEstimatedPosition();
   }
 

diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -226,6 +226,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
   // unless there have been no vision measurements after the last reset
   std::map<units::second_t, VisionUpdate> m_visionUpdates;
+
+  Pose2d m_poseEstimate;
 };
 }  // namespace frc
 

diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
@@ -14,7 +14,7 @@ PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
     Odometry<WheelSpeeds, WheelPositions>& odometry,
     const wpi::array<double, 3>& stateStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs)
-    : m_odometry(odometry) {
+    : m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) {
   for (size_t i = 0; i < 3; ++i) {
     m_q[i] = stateStdDevs[i] * stateStdDevs[i];
   }
@@ -50,11 +50,13 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
   m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
   m_odometryPoseBuffer.Clear();
   m_visionUpdates.clear();
+  m_poseEstimate = m_odometry.GetPose();
 }
 
 template <typename WheelSpeeds, typename WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
     const {
+  return m_poseEstimate;
   if (m_visionUpdates.empty()) {
     return m_odometry.GetPose();
   }
@@ -188,6 +190,10 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
   // Step 8: Remove later vision measurements. (Matches previous behavior)
   auto firstAfter = m_visionUpdates.upper_bound(timestamp);
   m_visionUpdates.erase(firstAfter, m_visionUpdates.end());
+
+  // Step 9: Update latest pose estimate. Since we cleared all updates after
+  // this vision update, it's guaranteed to be the latest vision update.
+  m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
 }
 
 template <typename WheelSpeeds, typename WheelPositions>
@@ -205,6 +211,13 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
 
   m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate);
 
+  if (m_visionUpdates.empty()) {
+    m_poseEstimate = odometryEstimate;
+  } else {
+    auto visionUpdate = m_visionUpdates.rbegin()->second;
+    m_poseEstimate = visionUpdate.Compensate(odometryEstimate);
+  }
+
   return GetEstimatedPosition();
 }