Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Discard stale pose estimates (#5045)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2023-02-04 01:04:30 -05:00
committed by GitHub
parent fe5d226a19
commit 4079eabe9b
14 changed files with 283 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -40,8 +40,10 @@ public class DifferentialDrivePoseEstimator {
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(1.5);
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
/**
* Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
@@ -187,6 +189,11 @@ public class DifferentialDrivePoseEstimator {
* or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);

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

@@ -39,8 +39,10 @@ public class MecanumDrivePoseEstimator {
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(1.5);
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
/**
* Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
@@ -175,6 +177,11 @@ public class MecanumDrivePoseEstimator {
* your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);

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

@@ -40,8 +40,10 @@ public class SwerveDrivePoseEstimator {
private final int m_numModules;
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(1.5);
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
/**
* Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
@@ -177,6 +179,11 @@ public class SwerveDrivePoseEstimator {
* or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);

7
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -94,6 +94,13 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Get the estimated pose from when the vision measurement was made.
auto sample = m_poseBuffer.Sample(timestamp);

7
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -105,6 +105,13 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Get the estimated pose from when the vision measurement was made.
auto sample = m_poseBuffer.Sample(timestamp);

6
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -240,14 +240,16 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
DifferentialDriveKinematics& m_kinematics;
DifferentialDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};

6
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -232,14 +232,16 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
MecanumDriveKinematics& m_kinematics;
MecanumDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};

13
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -170,6 +170,13 @@ class SwerveDrivePoseEstimator {
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Get the estimated pose from when the vision measurement was made.
auto sample = m_poseBuffer.Sample(timestamp);
@@ -374,14 +381,16 @@ class SwerveDrivePoseEstimator {
}
};
static constexpr units::second_t kBufferDuration = 1.5_s;
SwerveDriveKinematics<NumModules>& m_kinematics;
SwerveDriveOdometry<NumModules> m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};