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 7918f9fbb8e..c235420eace 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
@@ -27,6 +27,7 @@
*
{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
* you want; if you never call it then this class will behave exactly like regular encoder odometry.
*/
+@SuppressWarnings("PMD.UnusedFormalParameter")
public class DifferentialDrivePoseEstimator extends PoseEstimator {
/**
* Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
@@ -82,7 +83,6 @@ public DifferentialDrivePoseEstimator(
Matrix stateStdDevs,
Matrix visionMeasurementStdDevs) {
super(
- kinematics,
new DifferentialDriveOdometry(
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
stateStdDevs,
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 59bbb3235cd..d4cbd66d174 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
@@ -75,7 +75,6 @@ public MecanumDrivePoseEstimator(
Matrix stateStdDevs,
Matrix visionMeasurementStdDevs) {
super(
- kinematics,
new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
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
index 1d9a54b931a..fd187a30d3b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -10,17 +10,15 @@
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.Transform2d;
import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
-import edu.wpi.first.math.kinematics.Kinematics;
import edu.wpi.first.math.kinematics.Odometry;
import edu.wpi.first.math.kinematics.WheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
-import java.util.Map;
import java.util.NoSuchElementException;
-import java.util.Objects;
+import java.util.Optional;
/**
* This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
@@ -36,19 +34,19 @@
* never call it then this class will behave exactly like regular encoder odometry.
*/
public class PoseEstimator> {
- private final Kinematics, T> m_kinematics;
private final Odometry m_odometry;
private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1());
private final Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
- private final TimeInterpolatableBuffer m_poseBuffer =
+ private final TimeInterpolatableBuffer m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
+ private Pose2d m_poseEstimate;
+
/**
* Constructs a PoseEstimator.
*
- * @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param odometry A correctly-configured odometry object for your drivetrain.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
@@ -58,13 +56,11 @@ public class PoseEstimator> {
* the vision pose measurement less.
*/
public PoseEstimator(
- Kinematics, T> kinematics,
- Odometry odometry,
- Matrix stateStdDevs,
- Matrix visionMeasurementStdDevs) {
- m_kinematics = kinematics;
+ Odometry odometry, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) {
m_odometry = odometry;
+ m_poseEstimate = odometry.getPoseMeters();
+
for (int i = 0; i < 3; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
@@ -112,6 +108,7 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
m_poseBuffer.clear();
+ m_poseEstimate = m_odometry.getPoseMeters();
}
/**
@@ -120,7 +117,19 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
- return m_odometry.getPoseMeters();
+ return m_poseEstimate;
+ }
+
+ /**
+ * Return the pose at a given timestamp, if one exists.
+ *
+ * @param timestamp The pose's timestamp in seconds.
+ * @return The pose at a given timestamp, if one exists.
+ */
+ public Optional sampleAt(double timestamp) {
+ return m_poseBuffer
+ .getSample(timestamp)
+ .map(sample -> sample.plus(new Transform2d(m_odometry.getPoseMeters(), m_poseEstimate)));
}
/**
@@ -160,35 +169,27 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS
return;
}
- // Step 2: Measure the twist between the odometry pose and the vision pose.
- var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+ // Step 2: Record the odometry updates that have occurred since the vision measurement
+ var odometry_backtrack = new Transform2d(m_odometry.getPoseMeters(), sample.get());
+ var odometry_forward = new Transform2d(sample.get(), m_odometry.getPoseMeters());
+
+ // Step 3: Revert said odometry updates to get a state estimate from when the vision
+ // measurement occurred.
+ var old_estimate = m_poseEstimate.plus(odometry_backtrack);
- // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
+ // Step 4: Measure the twist between the odometry pose and the vision pose.
+ var twist = old_estimate.log(visionRobotPoseMeters);
+
+ // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
- // Step 4: Convert back to Twist2d.
+ // Step 6: Convert back to Twist2d.
var scaledTwist =
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.resetPosition(
- sample.get().gyroAngle,
- sample.get().wheelPositions,
- sample.get().poseMeters.exp(scaledTwist));
-
- // Step 6: Record the current pose to allow multiple measurements from the same timestamp
- m_poseBuffer.addSample(
- timestampSeconds,
- new InterpolationRecord(
- getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
-
- // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
- // pose buffer and correct odometry.
- for (Map.Entry entry :
- m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
- updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
- }
+ // Step 7: Apply this adjustment to the old estimate, then replay odometry updates.
+ m_poseEstimate = old_estimate.exp(scaledTwist).plus(odometry_forward);
}
/**
@@ -247,87 +248,12 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) {
- m_odometry.update(gyroAngle, wheelPositions);
- m_poseBuffer.addSample(
- currentTimeSeconds,
- new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()));
+ var lastOdom = m_odometry.getPoseMeters();
+ var currOdom = m_odometry.update(gyroAngle, wheelPositions);
+ m_poseBuffer.addSample(currentTimeSeconds, currOdom);
- return getEstimatedPosition();
- }
-
- /**
- * Represents an odometry record. The record contains the inputs provided as well as the pose that
- * was observed based on these inputs, as well as the previous record and its inputs.
- */
- private class InterpolationRecord implements Interpolatable {
- // The pose observed given the current sensor inputs and the previous pose.
- private final Pose2d poseMeters;
+ m_poseEstimate = m_poseEstimate.exp(lastOdom.log(currOdom));
- // The current gyro angle.
- private final Rotation2d gyroAngle;
-
- // The current encoder readings.
- private final T wheelPositions;
-
- /**
- * Constructs an Interpolation Record with the specified parameters.
- *
- * @param poseMeters The pose observed given the current sensor inputs and the previous pose.
- * @param gyro The current gyro angle.
- * @param wheelPositions The current encoder readings.
- */
- private InterpolationRecord(Pose2d poseMeters, Rotation2d gyro, T wheelPositions) {
- this.poseMeters = poseMeters;
- this.gyroAngle = gyro;
- this.wheelPositions = wheelPositions;
- }
-
- /**
- * Return the interpolated record. This object is assumed to be the starting position, or lower
- * bound.
- *
- * @param endValue The upper bound, or end.
- * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
- * @return The interpolated value.
- */
- @Override
- public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
- if (t < 0) {
- return this;
- } else if (t >= 1) {
- return endValue;
- } else {
- // Find the new wheel distances.
- var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
-
- // Find the new gyro angle.
- var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
- // Create a twist to represent the change based on the interpolated sensor inputs.
- Twist2d twist = m_kinematics.toTwist2d(wheelPositions, wheelLerp);
- twist.dtheta = gyroLerp.minus(gyroAngle).getRadians();
-
- return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp);
- }
- }
-
- @Override
- public boolean equals(Object obj) {
- if (this == obj) {
- return true;
- }
- if (!(obj instanceof PoseEstimator.InterpolationRecord)) {
- return false;
- }
- var record = (PoseEstimator>.InterpolationRecord) obj;
- return Objects.equals(gyroAngle, record.gyroAngle)
- && Objects.equals(wheelPositions, record.wheelPositions)
- && Objects.equals(poseMeters, record.poseMeters);
- }
-
- @Override
- public int hashCode() {
- return Objects.hash(gyroAngle, wheelPositions, poseMeters);
- }
+ return getEstimatedPosition();
}
}
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 f778b621e32..ae406b162cb 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
@@ -77,7 +77,6 @@ public SwerveDrivePoseEstimator(
Matrix stateStdDevs,
Matrix visionMeasurementStdDevs) {
super(
- kinematics,
new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 213ba991760..69e08360aec 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -21,5 +21,5 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
const wpi::array& visionMeasurementStdDevs)
: PoseEstimator(
- kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+ m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 7c75e71ffee..71da428440b 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -25,5 +25,5 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const wpi::array& stateStdDevs,
const wpi::array& visionMeasurementStdDevs)
: PoseEstimator(
- kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+ m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index eb437a40f7e..abfc35ce7b2 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -4,14 +4,16 @@
#pragma once
+#include
+
#include
#include
#include
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Twist2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
-#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/Odometry.h"
#include "frc/kinematics/WheelPositions.h"
#include "units/time.h"
@@ -37,8 +39,6 @@ class WPILIB_DLLEXPORT PoseEstimator {
/**
* Constructs a PoseEstimator.
*
- * @param kinematics A correctly-configured kinematics object for your
- * drivetrain.
* @param odometry A correctly-configured odometry object for your drivetrain.
* @param stateStdDevs Standard deviations of the pose estimate (x position in
* meters, y position in meters, and heading in radians). Increase these
@@ -48,8 +48,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
- PoseEstimator(Kinematics& kinematics,
- Odometry& odometry,
+ PoseEstimator(Odometry& odometry,
const wpi::array& stateStdDevs,
const wpi::array& visionMeasurementStdDevs);
@@ -86,6 +85,14 @@ class WPILIB_DLLEXPORT PoseEstimator {
*/
Pose2d GetEstimatedPosition() const;
+ /**
+ * Return the pose at a given timestamp, if one exists.
+ *
+ * @param timestamp The pose's timestamp.
+ * @return The pose at a given timestamp, if one exists.
+ */
+ std::optional SampleAt(units::second_t timestamp) const;
+
/**
* Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
@@ -171,57 +178,25 @@ class WPILIB_DLLEXPORT PoseEstimator {
const WheelPositions& wheelPositions);
private:
- struct InterpolationRecord {
- // The pose observed given the current sensor inputs and the previous pose.
- Pose2d pose;
-
- // The current gyroscope angle.
- Rotation2d gyroAngle;
-
- // The distances traveled by the wheels.
- WheelPositions wheelPositions;
-
- /**
- * Checks equality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are equal.
- */
- bool operator==(const InterpolationRecord& other) const = default;
-
- /**
- * Checks inequality between this InterpolationRecord and another object.
- *
- * @param other The other object.
- * @return Whether the two objects are not equal.
- */
- bool operator!=(const InterpolationRecord& other) const = default;
-
- /**
- * Interpolates between two InterpolationRecords.
- *
- * @param endValue The end value for the interpolation.
- * @param i The interpolant (fraction).
- *
- * @return The interpolated state.
- */
- InterpolationRecord Interpolate(
- Kinematics& kinematics,
- InterpolationRecord endValue, double i) const;
- };
-
static constexpr units::second_t kBufferDuration = 1.5_s;
- Kinematics& m_kinematics;
Odometry& m_odometry;
wpi::array m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
- TimeInterpolatableBuffer m_poseBuffer{
- kBufferDuration, [this](const InterpolationRecord& start,
- const InterpolationRecord& end, double t) {
- return start.Interpolate(this->m_kinematics, end, t);
+ TimeInterpolatableBuffer m_poseBuffer{
+ kBufferDuration, [](const Pose2d& start, const Pose2d& end, double t) {
+ if (t <= 0) {
+ return start;
+ } else if (t >= 1) {
+ return end;
+ } else {
+ auto twist = start.Log(end);
+ Twist2d scaledTwist{twist.dx * t, twist.dy * t, twist.dtheta * t};
+ return start.Exp(scaledTwist);
+ }
}};
+ 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
index 79d71bcc566..aa6e7ea79d1 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
@@ -10,11 +10,10 @@ namespace frc {
template
PoseEstimator::PoseEstimator(
- Kinematics& kinematics,
Odometry& odometry,
const wpi::array& stateStdDevs,
const wpi::array& visionMeasurementStdDevs)
- : m_kinematics(kinematics), m_odometry(odometry) {
+ : m_odometry(odometry), m_poseEstimate{odometry.GetPose()} {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
@@ -49,12 +48,25 @@ void PoseEstimator::ResetPosition(
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_poseBuffer.Clear();
+ m_poseEstimate = m_odometry.GetPose();
}
template
Pose2d PoseEstimator::GetEstimatedPosition()
const {
- return m_odometry.GetPose();
+ return m_poseEstimate;
+}
+
+template
+std::optional PoseEstimator::SampleAt(
+ units::second_t timestamp) const {
+ // TODO Use C++23 std::optional::transform
+ std::optional sample = m_poseBuffer.Sample(timestamp);
+ if (sample) {
+ return *sample + Transform2d{m_odometry.GetPose(), m_poseEstimate};
+ } else {
+ return std::nullopt;
+ }
}
template
@@ -75,44 +87,33 @@ void PoseEstimator::AddVisionMeasurement(
return;
}
- // Step 2: Measure the twist between the odometry pose and the vision pose
- auto twist = sample.value().pose.Log(visionRobotPose);
+ // Step 2: Record the odometry updates that have occurred since the vision
+ // measurement
+ Transform2d odometry_backtrack{m_odometry.GetPose(), *sample};
+ Transform2d odometry_forward{*sample, m_odometry.GetPose()};
+
+ // Step 3: Revert said odometry updates to get a state estimate from when the
+ // vision measurement occurred.
+ auto old_estimate = m_poseEstimate + odometry_backtrack;
- // Step 3: We should not trust the twist entirely, so instead we scale this
+ // Step 4: Measure the twist between the odometry pose and the vision pose.
+ auto twist = old_estimate.Log(visionRobotPose);
+
+ // Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Eigen::Vector3d k_times_twist =
m_visionK *
Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
- // Step 4: Convert back to Twist2d
+ // Step 6: Convert back to Twist2d.
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
- // Step 5: Reset Odometry to state at sample with vision adjustment.
- m_odometry.ResetPosition(sample.value().gyroAngle,
- sample.value().wheelPositions,
- sample.value().pose.Exp(scaledTwist));
-
- // Step 6: Record the current pose to allow multiple measurements from the
- // same timestamp
- m_poseBuffer.AddSample(timestamp,
- {GetEstimatedPosition(), sample.value().gyroAngle,
- sample.value().wheelPositions});
-
- // Step 7: Replay odometry inputs between sample time and latest recorded
- // sample to update the pose buffer and correct odometry.
- auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
- auto upper_bound =
- std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
- [](const auto& pair, auto t) { return t > pair.first; });
-
- for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
- UpdateWithTime(entry->first, entry->second.gyroAngle,
- entry->second.wheelPositions);
- }
+ // Step 7: Apply this adjustment to the old estimate, then replay dometry
+ // updates.
+ m_poseEstimate = old_estimate.Exp(scaledTwist) + odometry_forward;
}
template
@@ -126,40 +127,13 @@ template
Pose2d PoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions) {
- m_odometry.Update(gyroAngle, wheelPositions);
-
- WheelPositions internalWheelPositions = wheelPositions;
+ auto lastOdom = m_odometry.GetPose();
+ auto currOdom = m_odometry.Update(gyroAngle, wheelPositions);
+ m_poseBuffer.AddSample(currentTime, currOdom);
- m_poseBuffer.AddSample(
- currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
+ m_poseEstimate = m_poseEstimate.Exp(lastOdom.Log(currOdom));
return GetEstimatedPosition();
}
-template
-typename PoseEstimator::InterpolationRecord
-PoseEstimator::InterpolationRecord::Interpolate(
- Kinematics& kinematics,
- InterpolationRecord endValue, double i) const {
- if (i < 0) {
- return *this;
- } else if (i > 1) {
- return endValue;
- } else {
- // Find the new wheel distance measurements.
- WheelPositions wheels_lerp =
- this->wheelPositions.Interpolate(endValue.wheelPositions, i);
-
- // Find the new gyro angle.
- auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
- // Create a twist to represent the change based on the interpolated
- // sensor inputs.
- auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp);
- twist.dtheta = (gyro - gyroAngle).Radians();
-
- return {pose.Exp(twist), gyro, wheels_lerp};
- }
-}
-
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index 43831d7aff4..a43ee45a769 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -85,7 +85,7 @@ class SwerveDrivePoseEstimator
const wpi::array& visionMeasurementStdDevs)
: PoseEstimator,
SwerveDriveWheelPositions>(
- kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+ m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
/**
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index 94d28180d8f..bfb1b548976 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -100,7 +100,7 @@ class TimeInterpolatableBuffer {
*
* @param time The time at which to sample the buffer.
*/
- std::optional Sample(units::second_t time) {
+ std::optional Sample(units::second_t time) const {
if (m_pastSnapshots.empty()) {
return {};
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
deleted file mode 100644
index ea08a5f1d90..00000000000
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ /dev/null
@@ -1,324 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.math.estimator;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-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.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.math.trajectory.TrajectoryConfig;
-import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import java.util.List;
-import java.util.Random;
-import java.util.TreeMap;
-import java.util.function.Function;
-import org.junit.jupiter.api.Test;
-
-class MecanumDrivePoseEstimatorTest {
- @Test
- void testAccuracyFacingTrajectory() {
- var kinematics =
- new MecanumDriveKinematics(
- new Translation2d(1, 1), new Translation2d(1, -1),
- new Translation2d(-1, -1), new Translation2d(-1, 1));
-
- var wheelPositions = new MecanumDriveWheelPositions();
-
- var estimator =
- new MecanumDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- wheelPositions,
- new Pose2d(),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.45, 0.45, 0.1));
-
- var trajectory =
- TrajectoryGenerator.generateTrajectory(
- List.of(
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
- new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(2, 2));
-
- testFollowTrajectory(
- kinematics,
- estimator,
- trajectory,
- state ->
- new ChassisSpeeds(
- state.velocityMetersPerSecond,
- 0,
- state.velocityMetersPerSecond * state.curvatureRadPerMeter),
- state -> state.poseMeters,
- trajectory.getInitialPose(),
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- 0.02,
- 0.1,
- 0.25,
- true);
- }
-
- @Test
- void testBadInitialPose() {
- var kinematics =
- new MecanumDriveKinematics(
- new Translation2d(1, 1), new Translation2d(1, -1),
- new Translation2d(-1, -1), new Translation2d(-1, 1));
-
- var wheelPositions = new MecanumDriveWheelPositions();
-
- var estimator =
- new MecanumDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- wheelPositions,
- new Pose2d(),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.45, 0.45, 0.1));
-
- var trajectory =
- TrajectoryGenerator.generateTrajectory(
- List.of(
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
- new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(2, 2));
-
- for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
- for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
- var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
- var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
-
- var initial_pose =
- trajectory
- .getInitialPose()
- .plus(
- new Transform2d(
- new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
- heading_offset));
-
- testFollowTrajectory(
- kinematics,
- estimator,
- trajectory,
- state ->
- new ChassisSpeeds(
- state.velocityMetersPerSecond,
- 0,
- state.velocityMetersPerSecond * state.curvatureRadPerMeter),
- state -> state.poseMeters,
- initial_pose,
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- 0.02,
- 0.1,
- 0.25,
- false);
- }
- }
- }
-
- void testFollowTrajectory(
- final MecanumDriveKinematics kinematics,
- final MecanumDrivePoseEstimator estimator,
- final Trajectory trajectory,
- final Function chassisSpeedsGenerator,
- final Function visionMeasurementGenerator,
- final Pose2d startingPose,
- final Pose2d endingPose,
- final double dt,
- final double visionUpdateRate,
- final double visionUpdateDelay,
- final boolean checkError) {
- var wheelPositions = new MecanumDriveWheelPositions();
-
- estimator.resetPosition(new Rotation2d(), wheelPositions, startingPose);
-
- var rand = new Random(3538);
-
- double t = 0.0;
-
- final TreeMap visionUpdateQueue = new TreeMap<>();
-
- double maxError = Double.NEGATIVE_INFINITY;
- double errorSum = 0;
- while (t <= trajectory.getTotalTimeSeconds()) {
- var groundTruthState = trajectory.sample(t);
-
- // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
- // last vision measurement
- if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
- Pose2d newVisionPose =
- visionMeasurementGenerator
- .apply(groundTruthState)
- .plus(
- new Transform2d(
- new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.05)));
-
- visionUpdateQueue.put(t, newVisionPose);
- }
-
- // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
- // since it was measured
- if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
- var visionEntry = visionUpdateQueue.pollFirstEntry();
- estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
- }
-
- var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
-
- var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
-
- wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
- wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
- wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
- wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
-
- var xHat =
- estimator.updateWithTime(
- t,
- groundTruthState
- .poseMeters
- .getRotation()
- .plus(new Rotation2d(rand.nextGaussian() * 0.05))
- .minus(trajectory.getInitialPose().getRotation()),
- wheelPositions);
-
- double error =
- groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
- }
-
- assertEquals(
- endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
- assertEquals(
- endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
- assertEquals(
- endingPose.getRotation().getRadians(),
- estimator.getEstimatedPosition().getRotation().getRadians(),
- 0.15,
- "Incorrect Final Theta");
-
- if (checkError) {
- assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.2, "Incorrect max error");
- }
- }
-
- @Test
- void testSimultaneousVisionMeasurements() {
- // This tests for multiple vision measurements appled at the same time. The expected behavior
- // is that all measurements affect the estimated pose. The alternative result is that only one
- // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
- // estimated pose would converge to that measurement.
- var kinematics =
- new MecanumDriveKinematics(
- new Translation2d(1, 1), new Translation2d(1, -1),
- new Translation2d(-1, -1), new Translation2d(-1, 1));
-
- var wheelPositions = new MecanumDriveWheelPositions();
-
- var estimator =
- new MecanumDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- wheelPositions,
- new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.45, 0.45, 0.1));
-
- estimator.updateWithTime(0, new Rotation2d(), wheelPositions);
-
- var visionMeasurements =
- new Pose2d[] {
- new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
- new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
- new Pose2d(2, 4, Rotation2d.fromRadians(180)),
- };
-
- for (int i = 0; i < 1000; i++) {
- for (var measurement : visionMeasurements) {
- estimator.addVisionMeasurement(measurement, 0);
- }
- }
-
- for (var measurement : visionMeasurements) {
- var errorLog =
- "Estimator converged to one vision measurement: "
- + estimator.getEstimatedPosition().toString()
- + " -> "
- + measurement;
-
- var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
- var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
- var dtheta =
- Math.abs(
- measurement.getRotation().getDegrees()
- - estimator.getEstimatedPosition().getRotation().getDegrees());
-
- assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
- }
- }
-
- @Test
- void testDiscardsOldVisionMeasurements() {
- var kinematics =
- new MecanumDriveKinematics(
- new Translation2d(1, 1),
- new Translation2d(-1, 1),
- new Translation2d(1, -1),
- new Translation2d(-1, -1));
- var estimator =
- new MecanumDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- new MecanumDriveWheelPositions(),
- new Pose2d(),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.9, 0.9, 0.9));
-
- double time = 0;
-
- // Add enough measurements to fill up the buffer
- for (; time < 4; time += 0.02) {
- estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions());
- }
-
- var odometryPose = estimator.getEstimatedPosition();
-
- // Apply a vision measurement made 3 seconds ago
- // This test passes if this does not cause a ConcurrentModificationException.
- estimator.addVisionMeasurement(
- new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
- 1,
- VecBuilder.fill(0.1, 0.1, 0.1));
-
- assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
- assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
- assertEquals(
- odometryPose.getRotation().getRadians(),
- estimator.getEstimatedPosition().getRotation().getRadians(),
- "Incorrect Final Theta");
- }
-}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimationTestUtil.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimationTestUtil.java
new file mode 100644
index 00000000000..8458f8f2a09
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimationTestUtil.java
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.WheelPositions;
+
+public class PoseEstimationTestUtil {
+ /**
+ * This class is intended for use as "drivetrain-agnostic" kinematics. Odometry measurements are
+ * akin to Poses, while their difference is akin to Twists.
+ */
+ public static class SE2KinematicPrimitive implements WheelPositions {
+ private final Pose2d pose;
+
+ public SE2KinematicPrimitive(final Pose2d pose) {
+ this.pose = pose;
+ }
+
+ @Override
+ public SE2KinematicPrimitive copy() {
+ return this;
+ }
+
+ @Override
+ public SE2KinematicPrimitive interpolate(SE2KinematicPrimitive end, double t) {
+ var twist = this.pose.log(end.pose);
+ var scaled = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
+
+ return new SE2KinematicPrimitive(this.pose.exp(scaled));
+ }
+ }
+
+ public static class SE2Kinematics
+ implements Kinematics {
+ private final double timeConstant;
+
+ public SE2Kinematics(double timeConstant) {
+ this.timeConstant = timeConstant;
+ }
+
+ @Override
+ public ChassisSpeeds toChassisSpeeds(SE2KinematicPrimitive wheelSpeeds) {
+ var twist = new Pose2d().log(wheelSpeeds.pose);
+
+ return new ChassisSpeeds(
+ twist.dx * timeConstant, twist.dy * timeConstant, twist.dtheta * timeConstant);
+ }
+
+ @Override
+ public SE2KinematicPrimitive toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+ return new SE2KinematicPrimitive(
+ new Pose2d()
+ .exp(
+ new Twist2d(
+ chassisSpeeds.vxMetersPerSecond / timeConstant,
+ chassisSpeeds.vyMetersPerSecond / timeConstant,
+ chassisSpeeds.omegaRadiansPerSecond / timeConstant)));
+ }
+
+ @Override
+ public Twist2d toTwist2d(SE2KinematicPrimitive start, SE2KinematicPrimitive end) {
+ return start.pose.log(end.pose);
+ }
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java
similarity index 64%
rename from wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
rename to wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java
index 08c5a156cf1..4e04ddd3225 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java
@@ -8,35 +8,44 @@
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.estimator.PoseEstimationTestUtil.SE2KinematicPrimitive;
+import edu.wpi.first.math.estimator.PoseEstimationTestUtil.SE2Kinematics;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.Odometry;
+import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.util.SamplingUtil;
import java.util.List;
import java.util.Random;
import java.util.TreeMap;
-import java.util.function.Function;
import org.junit.jupiter.api.Test;
-class DifferentialDrivePoseEstimatorTest {
+class PoseEstimatorTest {
+ private final Vector stateStdDevs = VecBuilder.fill(0.05, 0.05, 0.05);
+ private final Vector stateNoiseStdDevs = stateStdDevs.div(50);
+ private final Vector visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.2);
+
+ private static final double visionUpdatePeriod = 0.2;
+ private static final double visionUpdateDelay = 0.1;
+
+ private static final double dt = 0.02;
+
@Test
void testAccuracy() {
- var kinematics = new DifferentialDriveKinematics(1);
+ var kinematics = new SE2Kinematics(dt);
+
+ var odometry =
+ new Odometry<>(
+ kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d());
+
+ var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs);
- var estimator =
- new DifferentialDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- 0,
- 0,
- new Pose2d(),
- VecBuilder.fill(0.02, 0.02, 0.01),
- VecBuilder.fill(0.1, 0.1, 0.1));
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
@@ -51,33 +60,25 @@ void testAccuracy() {
kinematics,
estimator,
trajectory,
- state ->
- new ChassisSpeeds(
- state.velocityMetersPerSecond,
- 0,
- state.velocityMetersPerSecond * state.curvatureRadPerMeter),
- state -> state.poseMeters,
trajectory.getInitialPose(),
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- 0.02,
- 0.1,
- 0.25,
- true);
+ trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters,
+ dt,
+ visionUpdatePeriod,
+ visionUpdateDelay,
+ true,
+ stateNoiseStdDevs,
+ visionStdDevs);
}
@Test
void testBadInitialPose() {
- var kinematics = new DifferentialDriveKinematics(1);
+ var kinematics = new SE2Kinematics(dt);
- var estimator =
- new DifferentialDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- 0,
- 0,
- new Pose2d(),
- VecBuilder.fill(0.02, 0.02, 0.01),
- VecBuilder.fill(0.1, 0.1, 0.1));
+ var odometry =
+ new Odometry<>(
+ kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d());
+
+ var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs);
var trajectory =
TrajectoryGenerator.generateTrajectory(
@@ -106,39 +107,34 @@ void testBadInitialPose() {
kinematics,
estimator,
trajectory,
- state ->
- new ChassisSpeeds(
- state.velocityMetersPerSecond,
- 0,
- state.velocityMetersPerSecond * state.curvatureRadPerMeter),
- state -> state.poseMeters,
initial_pose,
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- 0.02,
- 0.1,
- 0.25,
- false);
+ trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters,
+ dt,
+ visionUpdatePeriod,
+ visionUpdateDelay,
+ false,
+ stateNoiseStdDevs,
+ visionStdDevs);
}
}
}
void testFollowTrajectory(
- final DifferentialDriveKinematics kinematics,
- final DifferentialDrivePoseEstimator estimator,
+ final SE2Kinematics kinematics,
+ final PoseEstimator estimator,
final Trajectory trajectory,
- final Function chassisSpeedsGenerator,
- final Function visionMeasurementGenerator,
final Pose2d startingPose,
final Pose2d endingPose,
final double dt,
final double visionUpdateRate,
final double visionUpdateDelay,
- final boolean checkError) {
- double leftDistanceMeters = 0;
- double rightDistanceMeters = 0;
-
+ final boolean checkError,
+ final Vector stateStdDevs,
+ final Vector visionStdDevs) {
estimator.resetPosition(
- new Rotation2d(), leftDistanceMeters, rightDistanceMeters, startingPose);
+ trajectory.getInitialPose().getRotation(),
+ new SE2KinematicPrimitive(trajectory.getInitialPose()),
+ startingPose);
var rand = new Random(3538);
@@ -154,13 +150,8 @@ void testFollowTrajectory(
// We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
// last vision measurement
if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
- Pose2d newVisionPose =
- visionMeasurementGenerator
- .apply(groundTruthState)
- .plus(
- new Transform2d(
- new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.05)));
+ var adjustment = SamplingUtil.sampleTwist2d(rand, visionStdDevs);
+ Pose2d newVisionPose = groundTruthState.poseMeters.exp(adjustment);
visionUpdateQueue.put(t, newVisionPose);
}
@@ -169,26 +160,21 @@ void testFollowTrajectory(
// since it was measured
if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
var visionEntry = visionUpdateQueue.pollFirstEntry();
+
estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
}
- var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+ var gyroAngle =
+ groundTruthState
+ .poseMeters
+ .getRotation()
+ .plus(SamplingUtil.sampleRotation2d(rand, stateStdDevs.get(2, 0)));
- var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+ var update =
+ new SE2KinematicPrimitive(
+ groundTruthState.poseMeters.exp(SamplingUtil.sampleTwist2d(rand, stateStdDevs)));
- leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt;
- rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt;
-
- var xHat =
- estimator.updateWithTime(
- t,
- groundTruthState
- .poseMeters
- .getRotation()
- .plus(new Rotation2d(rand.nextGaussian() * 0.05))
- .minus(trajectory.getInitialPose().getRotation()),
- leftDistanceMeters,
- rightDistanceMeters);
+ var xHat = estimator.updateWithTime(t, gyroAngle, update);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
@@ -201,9 +187,9 @@ void testFollowTrajectory(
}
assertEquals(
- endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+ endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.15, "Incorrect Final X");
assertEquals(
- endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+ endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.15, "Incorrect Final Y");
assertEquals(
endingPose.getRotation().getRadians(),
estimator.getEstimatedPosition().getRotation().getRadians(),
@@ -212,8 +198,8 @@ void testFollowTrajectory(
if (checkError) {
assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+ 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
+ assertEquals(0.0, maxError, 0.3, "Incorrect max error");
}
}
@@ -223,19 +209,13 @@ void testSimultaneousVisionMeasurements() {
// is that all measurements affect the estimated pose. The alternative result is that only one
// vision measurement affects the outcome. If that were the case, after 1000 measurements, the
// estimated pose would converge to that measurement.
- var kinematics = new DifferentialDriveKinematics(1);
-
- var estimator =
- new DifferentialDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- 0,
- 0,
- new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
- VecBuilder.fill(0.02, 0.02, 0.01),
- VecBuilder.fill(0.1, 0.1, 0.1));
+ var kinematics = new SE2Kinematics(dt);
+ var odometry =
+ new Odometry<>(
+ kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d());
+ var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs);
- estimator.updateWithTime(0, new Rotation2d(), 0, 0);
+ estimator.updateWithTime(0, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()));
var visionMeasurements =
new Pose2d[] {
@@ -270,22 +250,17 @@ void testSimultaneousVisionMeasurements() {
@Test
void testDiscardsStaleVisionMeasurements() {
- var kinematics = new DifferentialDriveKinematics(1);
- var estimator =
- new DifferentialDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- 0,
- 0,
- new Pose2d(),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.9, 0.9, 0.9));
+ var kinematics = new SE2Kinematics(dt);
+ var odometry =
+ new Odometry<>(
+ kinematics, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()), new Pose2d());
+ var estimator = new PoseEstimator<>(odometry, stateStdDevs, visionStdDevs);
double time = 0;
// Add enough measurements to fill up the buffer
for (; time < 4; time += 0.02) {
- estimator.updateWithTime(time, new Rotation2d(), 0, 0);
+ estimator.updateWithTime(time, new Rotation2d(), new SE2KinematicPrimitive(new Pose2d()));
}
var odometryPose = estimator.getEstimatedPosition();
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
deleted file mode 100644
index f827a102527..00000000000
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ /dev/null
@@ -1,358 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.math.estimator;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-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.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.math.trajectory.TrajectoryConfig;
-import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import java.util.List;
-import java.util.Random;
-import java.util.TreeMap;
-import java.util.function.Function;
-import org.junit.jupiter.api.Test;
-
-class SwerveDrivePoseEstimatorTest {
- @Test
- void testAccuracyFacingTrajectory() {
- var kinematics =
- new SwerveDriveKinematics(
- new Translation2d(1, 1),
- new Translation2d(1, -1),
- new Translation2d(-1, -1),
- new Translation2d(-1, 1));
-
- var fl = new SwerveModulePosition();
- var fr = new SwerveModulePosition();
- var bl = new SwerveModulePosition();
- var br = new SwerveModulePosition();
-
- var estimator =
- new SwerveDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- new SwerveModulePosition[] {fl, fr, bl, br},
- new Pose2d(),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.5, 0.5, 0.5));
-
- var trajectory =
- TrajectoryGenerator.generateTrajectory(
- List.of(
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
- new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(2, 2));
-
- testFollowTrajectory(
- kinematics,
- estimator,
- trajectory,
- state ->
- new ChassisSpeeds(
- state.velocityMetersPerSecond,
- 0,
- state.velocityMetersPerSecond * state.curvatureRadPerMeter),
- state -> state.poseMeters,
- trajectory.getInitialPose(),
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- 0.02,
- 0.1,
- 0.25,
- true);
- }
-
- @Test
- void testBadInitialPose() {
- var kinematics =
- new SwerveDriveKinematics(
- new Translation2d(1, 1),
- new Translation2d(1, -1),
- new Translation2d(-1, -1),
- new Translation2d(-1, 1));
-
- var fl = new SwerveModulePosition();
- var fr = new SwerveModulePosition();
- var bl = new SwerveModulePosition();
- var br = new SwerveModulePosition();
-
- var estimator =
- new SwerveDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- new SwerveModulePosition[] {fl, fr, bl, br},
- new Pose2d(-1, -1, Rotation2d.fromRadians(-1)),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.9, 0.9, 0.9));
- var trajectory =
- TrajectoryGenerator.generateTrajectory(
- List.of(
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
- new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
- new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(2, 2));
-
- for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
- for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
- var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
- var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
-
- var initial_pose =
- trajectory
- .getInitialPose()
- .plus(
- new Transform2d(
- new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
- heading_offset));
-
- testFollowTrajectory(
- kinematics,
- estimator,
- trajectory,
- state ->
- new ChassisSpeeds(
- state.velocityMetersPerSecond,
- 0,
- state.velocityMetersPerSecond * state.curvatureRadPerMeter),
- state -> state.poseMeters,
- initial_pose,
- new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
- 0.02,
- 0.1,
- 1.0,
- false);
- }
- }
- }
-
- void testFollowTrajectory(
- final SwerveDriveKinematics kinematics,
- final SwerveDrivePoseEstimator estimator,
- final Trajectory trajectory,
- final Function chassisSpeedsGenerator,
- final Function visionMeasurementGenerator,
- final Pose2d startingPose,
- final Pose2d endingPose,
- final double dt,
- final double visionUpdateRate,
- final double visionUpdateDelay,
- final boolean checkError) {
- SwerveModulePosition[] positions = {
- new SwerveModulePosition(),
- new SwerveModulePosition(),
- new SwerveModulePosition(),
- new SwerveModulePosition()
- };
-
- estimator.resetPosition(new Rotation2d(), positions, startingPose);
-
- var rand = new Random(3538);
-
- double t = 0.0;
-
- final TreeMap visionUpdateQueue = new TreeMap<>();
-
- double maxError = Double.NEGATIVE_INFINITY;
- double errorSum = 0;
- while (t <= trajectory.getTotalTimeSeconds()) {
- var groundTruthState = trajectory.sample(t);
-
- // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
- // last vision measurement
- if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
- Pose2d newVisionPose =
- visionMeasurementGenerator
- .apply(groundTruthState)
- .plus(
- new Transform2d(
- new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.05)));
-
- visionUpdateQueue.put(t, newVisionPose);
- }
-
- // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
- // since it was measured
- if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
- var visionEntry = visionUpdateQueue.pollFirstEntry();
- estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
- }
-
- var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
-
- var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
-
- for (int i = 0; i < moduleStates.length; i++) {
- positions[i].distanceMeters +=
- moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt;
- positions[i].angle =
- moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
- }
-
- var xHat =
- estimator.updateWithTime(
- t,
- groundTruthState
- .poseMeters
- .getRotation()
- .plus(new Rotation2d(rand.nextGaussian() * 0.05))
- .minus(trajectory.getInitialPose().getRotation()),
- positions);
-
- double error =
- groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
- }
-
- assertEquals(
- endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
- assertEquals(
- endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
- assertEquals(
- endingPose.getRotation().getRadians(),
- estimator.getEstimatedPosition().getRotation().getRadians(),
- 0.15,
- "Incorrect Final Theta");
-
- if (checkError) {
- assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.2, "Incorrect max error");
- }
- }
-
- @Test
- void testSimultaneousVisionMeasurements() {
- // This tests for multiple vision measurements appled at the same time. The expected behavior
- // is that all measurements affect the estimated pose. The alternative result is that only one
- // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
- // estimated pose would converge to that measurement.
- var kinematics =
- new SwerveDriveKinematics(
- new Translation2d(1, 1),
- new Translation2d(1, -1),
- new Translation2d(-1, -1),
- new Translation2d(-1, 1));
-
- var fl = new SwerveModulePosition();
- var fr = new SwerveModulePosition();
- var bl = new SwerveModulePosition();
- var br = new SwerveModulePosition();
-
- var estimator =
- new SwerveDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- new SwerveModulePosition[] {fl, fr, bl, br},
- new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.9, 0.9, 0.9));
-
- estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br});
-
- var visionMeasurements =
- new Pose2d[] {
- new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
- new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
- new Pose2d(2, 4, Rotation2d.fromRadians(180)),
- };
-
- for (int i = 0; i < 1000; i++) {
- for (var measurement : visionMeasurements) {
- estimator.addVisionMeasurement(measurement, 0);
- }
- }
-
- for (var measurement : visionMeasurements) {
- var errorLog =
- "Estimator converged to one vision measurement: "
- + estimator.getEstimatedPosition().toString()
- + " -> "
- + measurement;
-
- var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
- var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
- var dtheta =
- Math.abs(
- measurement.getRotation().getDegrees()
- - estimator.getEstimatedPosition().getRotation().getDegrees());
-
- assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
- }
- }
-
- @Test
- void testDiscardsOldVisionMeasurements() {
- var kinematics =
- new SwerveDriveKinematics(
- new Translation2d(1, 1),
- new Translation2d(-1, 1),
- new Translation2d(1, -1),
- new Translation2d(-1, -1));
- var estimator =
- new SwerveDrivePoseEstimator(
- kinematics,
- new Rotation2d(),
- new SwerveModulePosition[] {
- new SwerveModulePosition(),
- new SwerveModulePosition(),
- new SwerveModulePosition(),
- new SwerveModulePosition()
- },
- new Pose2d(),
- VecBuilder.fill(0.1, 0.1, 0.1),
- VecBuilder.fill(0.9, 0.9, 0.9));
-
- double time = 0;
-
- // Add enough measurements to fill up the buffer
- for (; time < 4; time += 0.02) {
- estimator.updateWithTime(
- time,
- new Rotation2d(),
- new SwerveModulePosition[] {
- new SwerveModulePosition(),
- new SwerveModulePosition(),
- new SwerveModulePosition(),
- new SwerveModulePosition()
- });
- }
-
- var odometryPose = estimator.getEstimatedPosition();
-
- // Apply a vision measurement made 3 seconds ago
- // This test passes if this does not cause a ConcurrentModificationException.
- estimator.addVisionMeasurement(
- new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
- 1,
- VecBuilder.fill(0.1, 0.1, 0.1));
-
- assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
- assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
- assertEquals(
- odometryPose.getRotation().getRadians(),
- estimator.getEstimatedPosition().getRotation().getRadians(),
- "Incorrect Final Theta");
- }
-}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/util/SamplingUtil.java b/wpimath/src/test/java/edu/wpi/first/math/util/SamplingUtil.java
new file mode 100644
index 00000000000..132a73d5452
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/util/SamplingUtil.java
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.util;
+
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Random;
+
+public final class SamplingUtil {
+ private SamplingUtil() {}
+
+ public static Rotation2d sampleRotation2d(Random rand, double stdev) {
+ return Rotation2d.fromRadians(rand.nextGaussian() * stdev);
+ }
+
+ public static Twist2d sampleTwist2d(Random rand, Vector stdev) {
+ return new Twist2d(
+ rand.nextGaussian() * stdev.get(0, 0),
+ rand.nextGaussian() * stdev.get(1, 0),
+ sampleRotation2d(rand, stdev.get(2, 0)).getRadians());
+ }
+}
diff --git a/wpimath/src/test/native/cpp/SamplingUtil.h b/wpimath/src/test/native/cpp/SamplingUtil.h
new file mode 100644
index 00000000000..1fa3fd4857c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/SamplingUtil.h
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include
+
+#include
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Twist2d.h"
+
+namespace frc {
+
+template
+frc::Rotation2d SampleRotation2d(Generator& generator,
+ std::normal_distribution& distribution,
+ double stdDev) {
+ return frc::Rotation2d{units::radian_t{distribution(generator) * stdDev}};
+}
+
+template
+frc::Twist2d SampleTwist2d(Generator& generator,
+ std::normal_distribution& distribution,
+ wpi::array stdDev) {
+ return frc::Twist2d{units::meter_t{distribution(generator) * stdDev[0]},
+ units::meter_t{distribution(generator) * stdDev[1]},
+ units::radian_t{distribution(generator) * stdDev[2]}};
+}
+
+} // namespace frc
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
deleted file mode 100644
index 8b3a586892d..00000000000
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include
-#include
-#include
-
-#include
-
-#include "frc/estimator/MecanumDrivePoseEstimator.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-
-void testFollowTrajectory(
- const frc::MecanumDriveKinematics& kinematics,
- frc::MecanumDrivePoseEstimator& estimator,
- const frc::Trajectory& trajectory,
- std::function
- chassisSpeedsGenerator,
- std::function
- visionMeasurementGenerator,
- const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
- const units::second_t dt, const units::second_t kVisionUpdateRate,
- const units::second_t kVisionUpdateDelay, const bool checkError,
- const bool debug) {
- frc::MecanumDriveWheelPositions wheelPositions{};
-
- estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose);
-
- std::default_random_engine generator;
- std::normal_distribution distribution(0.0, 1.0);
-
- units::second_t t = 0_s;
-
- std::vector> visionPoses;
- std::vector>
- visionLog;
-
- double maxError = -std::numeric_limits::max();
- double errorSum = 0;
-
- if (debug) {
- fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
- }
-
- while (t < trajectory.TotalTime()) {
- frc::Trajectory::State groundTruthState = trajectory.Sample(t);
-
- // We are due for a new vision measurement if it's been `visionUpdateRate`
- // seconds since the last vision measurement
- if (visionPoses.empty() ||
- visionPoses.back().first + kVisionUpdateRate < t) {
- auto visionPose =
- visionMeasurementGenerator(groundTruthState) +
- frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m},
- frc::Rotation2d{distribution(generator) * 0.05_rad}};
- visionPoses.push_back({t, visionPose});
- }
-
- // We should apply the oldest vision measurement if it has been
- // `visionUpdateDelay` seconds since it was measured
- if (!visionPoses.empty() &&
- visionPoses.front().first + kVisionUpdateDelay < t) {
- auto visionEntry = visionPoses.front();
- estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
- visionPoses.erase(visionPoses.begin());
- visionLog.push_back({t, visionEntry.first, visionEntry.second});
- }
-
- auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
-
- auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
-
- wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
- wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
- wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
- wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
-
- auto xhat = estimator.UpdateWithTime(
- t,
- groundTruthState.pose.Rotation() +
- frc::Rotation2d{distribution(generator) * 0.05_rad} -
- trajectory.InitialPose().Rotation(),
- wheelPositions);
-
- if (debug) {
- fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
- xhat.Y().value(), xhat.Rotation().Radians().value(),
- groundTruthState.pose.X().value(),
- groundTruthState.pose.Y().value(),
- groundTruthState.pose.Rotation().Radians().value());
- }
-
- double error = groundTruthState.pose.Translation()
- .Distance(xhat.Translation())
- .value();
-
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
- }
-
- if (debug) {
- fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
-
- units::second_t apply_time;
- units::second_t measure_time;
- frc::Pose2d vision_pose;
- for (auto record : visionLog) {
- std::tie(apply_time, measure_time, vision_pose) = record;
- fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
- measure_time.value(), vision_pose.X().value(),
- vision_pose.Y().value(),
- vision_pose.Rotation().Radians().value());
- }
- }
-
- EXPECT_NEAR(endingPose.X().value(),
- estimator.GetEstimatedPosition().X().value(), 0.08);
- EXPECT_NEAR(endingPose.Y().value(),
- estimator.GetEstimatedPosition().Y().value(), 0.08);
- EXPECT_NEAR(endingPose.Rotation().Radians().value(),
- estimator.GetEstimatedPosition().Rotation().Radians().value(),
- 0.15);
-
- if (checkError) {
- // NOLINTNEXTLINE(bugprone-integer-division)
- EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
- EXPECT_LT(maxError, 0.2);
- }
-}
-
-TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
- frc::MecanumDriveKinematics kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::MecanumDriveWheelPositions wheelPositions;
-
- frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
- wheelPositions, frc::Pose2d{},
- {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
-
- frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 135_deg},
- frc::Pose2d{-3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 45_deg}},
- frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
-
- testFollowTrajectory(
- kinematics, estimator, trajectory,
- [&](frc::Trajectory::State& state) {
- return frc::ChassisSpeeds{state.velocity, 0_mps,
- state.velocity * state.curvature};
- },
- [&](frc::Trajectory::State& state) { return state.pose; },
- trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
- 0.1_s, 0.25_s, true, false);
-}
-
-TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
- frc::MecanumDriveKinematics kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::MecanumDriveWheelPositions wheelPositions;
-
- frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
- wheelPositions, frc::Pose2d{},
- {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
-
- frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 135_deg},
- frc::Pose2d{-3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 45_deg}},
- frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
-
- for (units::degree_t offset_direction_degs = 0_deg;
- offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
- for (units::degree_t offset_heading_degs = 0_deg;
- offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
- auto pose_offset = frc::Rotation2d{offset_direction_degs};
- auto heading_offset = frc::Rotation2d{offset_heading_degs};
-
- auto initial_pose =
- trajectory.InitialPose() +
- frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
- pose_offset.Sin() * 1_m},
- heading_offset};
-
- testFollowTrajectory(
- kinematics, estimator, trajectory,
- [&](frc::Trajectory::State& state) {
- return frc::ChassisSpeeds{state.velocity, 0_mps,
- state.velocity * state.curvature};
- },
- [&](frc::Trajectory::State& state) { return state.pose; },
- initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
- 0.25_s, false, false);
- }
- }
-}
-
-TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
- // This tests for multiple vision measurements appled at the same time.
- // The expected behavior is that all measurements affect the estimated pose.
- // The alternative result is that only one vision measurement affects the
- // outcome. If that were the case, after 1000 measurements, the estimated
- // pose would converge to that measurement.
- frc::MecanumDriveKinematics kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::MecanumDriveWheelPositions wheelPositions;
-
- frc::MecanumDrivePoseEstimator estimator{
- kinematics, frc::Rotation2d{},
- wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
- {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
-
- estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions);
-
- for (int i = 0; i < 1000; i++) {
- estimator.AddVisionMeasurement(
- frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
- estimator.AddVisionMeasurement(
- frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
- estimator.AddVisionMeasurement(
- frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
- }
-
- {
- auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
- auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
- auto dtheta = units::math::abs(
- estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
-
- EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
- }
-
- {
- auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
- auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
- auto dtheta = units::math::abs(
- estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
-
- EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
- }
-
- {
- auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
- auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
- auto dtheta = units::math::abs(
- estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
-
- EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
- }
-}
-
-TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
- frc::MecanumDriveKinematics kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::MecanumDrivePoseEstimator estimator{
- kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
- frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
-
- // Add enough measurements to fill up the buffer
- for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
- estimator.UpdateWithTime(time, frc::Rotation2d{},
- frc::MecanumDriveWheelPositions{});
- }
-
- auto odometryPose = estimator.GetEstimatedPosition();
-
- // Apply a vision measurement from 3 seconds ago
- estimator.AddVisionMeasurement(
- frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
- 1_s, {0.1, 0.1, 0.1});
-
- EXPECT_NEAR(odometryPose.X().value(),
- estimator.GetEstimatedPosition().X().value(), 1e-6);
- EXPECT_NEAR(odometryPose.Y().value(),
- estimator.GetEstimatedPosition().Y().value(), 1e-6);
- EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
- estimator.GetEstimatedPosition().Rotation().Radians().value(),
- 1e-6);
-}
diff --git a/wpimath/src/test/native/cpp/estimator/PoseEstimationTestUtil.h b/wpimath/src/test/native/cpp/estimator/PoseEstimationTestUtil.h
new file mode 100644
index 00000000000..093a63a0368
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/PoseEstimationTestUtil.h
@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Twist2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/WheelPositions.h"
+
+namespace frc {
+
+class SE2KinematicPrimitive {
+ public:
+ explicit constexpr SE2KinematicPrimitive(const Pose2d& pose) : m_pose{pose} {}
+ constexpr SE2KinematicPrimitive(const SE2KinematicPrimitive&) = default;
+
+ SE2KinematicPrimitive& operator=(const SE2KinematicPrimitive&) = default;
+
+ SE2KinematicPrimitive Interpolate(const SE2KinematicPrimitive& endValue,
+ double t) const {
+ auto twist = m_pose.Log(endValue.m_pose);
+ Twist2d scaled{twist.dx * t, twist.dy * t, twist.dtheta * t};
+
+ return SE2KinematicPrimitive{m_pose.Exp(scaled)};
+ }
+
+ constexpr const Pose2d& GetPose() const { return m_pose; }
+
+ private:
+ Pose2d m_pose;
+};
+
+static_assert(frc::WheelPositions);
+
+class SE2Kinematics
+ : public Kinematics {
+ public:
+ explicit SE2Kinematics(units::second_t dt) : m_dt{dt} {}
+
+ ChassisSpeeds ToChassisSpeeds(
+ const SE2KinematicPrimitive& wheelSpeeds) const override {
+ auto twist = Pose2d{}.Log(wheelSpeeds.GetPose());
+ return ChassisSpeeds{twist.dx / m_dt, twist.dy / m_dt, twist.dtheta / m_dt};
+ }
+
+ SE2KinematicPrimitive ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const override {
+ return SE2KinematicPrimitive{
+ Pose2d{}.Exp(Twist2d{chassisSpeeds.vx * m_dt, chassisSpeeds.vy * m_dt,
+ chassisSpeeds.omega * m_dt})};
+ }
+
+ Twist2d ToTwist2d(const SE2KinematicPrimitive& start,
+ const SE2KinematicPrimitive& end) const override {
+ return start.GetPose().Log(end.GetPose());
+ }
+
+ private:
+ units::second_t m_dt;
+};
+
+} // namespace frc
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/PoseEstimatorTest.cpp
similarity index 60%
rename from wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
rename to wpimath/src/test/native/cpp/estimator/PoseEstimatorTest.cpp
index 3103068344d..8c6975c7b9d 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/PoseEstimatorTest.cpp
@@ -7,34 +7,40 @@
#include
#include
+#include
#include
+#include "../SamplingUtil.h"
+#include "PoseEstimationTestUtil.h"
#include "frc/StateSpaceUtil.h"
-#include "frc/estimator/DifferentialDrivePoseEstimator.h"
+#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/Odometry.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
-void testFollowTrajectory(
- const frc::DifferentialDriveKinematics& kinematics,
- frc::DifferentialDrivePoseEstimator& estimator,
- const frc::Trajectory& trajectory,
- std::function
- chassisSpeedsGenerator,
- std::function
- visionMeasurementGenerator,
- const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
- const units::second_t dt, const units::second_t kVisionUpdateRate,
- const units::second_t kVisionUpdateDelay, const bool checkError,
- const bool debug) {
- units::meter_t leftDistance = 0_m;
- units::meter_t rightDistance = 0_m;
-
- estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance,
+namespace frc {
+
+using SE2Odometry = Odometry;
+using SE2PoseEstimator =
+ PoseEstimator;
+
+} // namespace frc
+
+void testFollowTrajectory(const frc::SE2Kinematics& kinematics,
+ frc::SE2PoseEstimator& estimator,
+ const frc::Trajectory& trajectory,
+ const frc::Pose2d& startingPose,
+ const frc::Pose2d& endingPose,
+ const units::second_t dt,
+ const units::second_t kVisionUpdateRate,
+ const units::second_t kVisionUpdateDelay,
+ const bool checkError, const bool debug) {
+ frc::SE2KinematicPrimitive wheelPositions{frc::Pose2d{}};
+ estimator.ResetPosition(trajectory.InitialPose().Rotation(), wheelPositions,
startingPose);
std::default_random_engine generator;
@@ -50,9 +56,7 @@ void testFollowTrajectory(
double errorSum = 0;
if (debug) {
- fmt::print(
- "time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, "
- "right\n");
+ fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
}
while (t < trajectory.TotalTime()) {
@@ -62,11 +66,9 @@ void testFollowTrajectory(
// seconds since the last vision measurement
if (visionPoses.empty() ||
visionPoses.back().first + kVisionUpdateRate < t) {
- auto visionPose =
- visionMeasurementGenerator(groundTruthState) +
- frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m},
- frc::Rotation2d{distribution(generator) * 0.05_rad}};
+ auto adjustment =
+ frc::SampleTwist2d(generator, distribution, {0.1, 0.1, 0.05});
+ auto visionPose = groundTruthState.pose.Exp(adjustment);
visionPoses.push_back({t, visionPose});
}
@@ -80,27 +82,25 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
- auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
+ auto gyroAngle = groundTruthState.pose.Rotation() +
+ frc::SampleRotation2d(generator, distribution, 0.05);
- auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+ frc::Twist2d trueDelta{
+ groundTruthState.velocity * dt, 0_m,
+ groundTruthState.velocity * groundTruthState.curvature * dt};
- leftDistance += wheelSpeeds.left * dt;
- rightDistance += wheelSpeeds.right * dt;
+ wheelPositions = frc::SE2KinematicPrimitive{
+ wheelPositions.GetPose().Exp(trueDelta).Exp(frc::SampleTwist2d(
+ generator, distribution, {0.001, 0.001, 0.005}))};
- auto xhat = estimator.UpdateWithTime(
- t,
- groundTruthState.pose.Rotation() +
- frc::Rotation2d{distribution(generator) * 0.05_rad} -
- trajectory.InitialPose().Rotation(),
- leftDistance, rightDistance);
+ auto xhat = estimator.UpdateWithTime(t, gyroAngle, wheelPositions);
if (debug) {
- fmt::print(
- "{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
- xhat.Y().value(), xhat.Rotation().Radians().value(),
- groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
- groundTruthState.pose.Rotation().Radians().value(),
- leftDistance.value(), rightDistance.value());
+ fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+ xhat.Y().value(), xhat.Rotation().Radians().value(),
+ groundTruthState.pose.X().value(),
+ groundTruthState.pose.Y().value(),
+ groundTruthState.pose.Rotation().Radians().value());
}
double error = groundTruthState.pose.Translation()
@@ -131,12 +131,12 @@ void testFollowTrajectory(
}
EXPECT_NEAR(endingPose.X().value(),
- estimator.GetEstimatedPosition().X().value(), 0.08);
+ estimator.GetEstimatedPosition().X().value(), 0.06);
EXPECT_NEAR(endingPose.Y().value(),
- estimator.GetEstimatedPosition().Y().value(), 0.08);
+ estimator.GetEstimatedPosition().Y().value(), 0.06);
EXPECT_NEAR(endingPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition().Rotation().Radians().value(),
- 0.15);
+ 0.10);
if (checkError) {
// NOLINTNEXTLINE(bugprone-integer-division)
@@ -145,12 +145,13 @@ void testFollowTrajectory(
}
}
-TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
- frc::DifferentialDriveKinematics kinematics{1.0_m};
-
- frc::DifferentialDrivePoseEstimator estimator{
- kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
- {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+TEST(PoseEstimatorTest, Accuracy) {
+ frc::SE2Kinematics kinematics{0.02_s};
+ frc::SE2Odometry odometry{kinematics, frc::Rotation2d{},
+ frc::SE2KinematicPrimitive{frc::Pose2d{}},
+ frc::Pose2d{}};
+ frc::SE2PoseEstimator estimator{
+ odometry, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
@@ -159,23 +160,19 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(2_mps, 2_mps_sq));
- testFollowTrajectory(
- kinematics, estimator, trajectory,
- [&](frc::Trajectory::State& state) {
- return frc::ChassisSpeeds{state.velocity, 0_mps,
- state.velocity * state.curvature};
- },
- [&](frc::Trajectory::State& state) { return state.pose; },
- trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
- 0.1_s, 0.25_s, true, false);
+ testFollowTrajectory(kinematics, estimator, trajectory,
+ trajectory.InitialPose(),
+ trajectory.Sample(trajectory.TotalTime()).pose, 0.02_s,
+ 0.1_s, 0.25_s, true, false);
}
-TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
- frc::DifferentialDriveKinematics kinematics{1.0_m};
-
- frc::DifferentialDrivePoseEstimator estimator{
- kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
- {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+TEST(PoseEstimatorTest, BadInitialPose) {
+ frc::SE2Kinematics kinematics{0.02_s};
+ frc::SE2Odometry odometry{kinematics, frc::Rotation2d{},
+ frc::SE2KinematicPrimitive{frc::Pose2d{}},
+ frc::Pose2d{}};
+ frc::SE2PoseEstimator estimator{
+ odometry, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
@@ -197,37 +194,28 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
pose_offset.Sin() * 1_m},
heading_offset};
- testFollowTrajectory(
- kinematics, estimator, trajectory,
- [&](frc::Trajectory::State& state) {
- return frc::ChassisSpeeds{state.velocity, 0_mps,
- state.velocity * state.curvature};
- },
- [&](frc::Trajectory::State& state) { return state.pose; },
- initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
- 0.25_s, false, false);
+ testFollowTrajectory(kinematics, estimator, trajectory, initial_pose,
+ trajectory.Sample(trajectory.TotalTime()).pose,
+ 0.02_s, 0.1_s, 0.25_s, false, false);
}
}
}
-TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+TEST(PoseEstimatorTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements appled at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
- frc::DifferentialDriveKinematics kinematics{1.0_m};
+ frc::SE2Kinematics kinematics{0.02_s};
+ frc::SE2Odometry odometry{kinematics, frc::Rotation2d{},
+ frc::SE2KinematicPrimitive{frc::Pose2d{}},
+ frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}};
+ frc::SE2PoseEstimator estimator{
+ odometry, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
- frc::DifferentialDrivePoseEstimator estimator{
- kinematics,
- frc::Rotation2d{},
- 0_m,
- 0_m,
- frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
- {0.02, 0.02, 0.01},
- {0.1, 0.1, 0.1}};
-
- estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m);
+ estimator.UpdateWithTime(0_s, frc::Rotation2d{},
+ frc::SE2KinematicPrimitive{frc::Pose2d{}});
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
@@ -266,16 +254,18 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
}
-TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
- frc::DifferentialDriveKinematics kinematics{1_m};
-
- frc::DifferentialDrivePoseEstimator estimator{
- kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
- {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+TEST(PoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+ frc::SE2Kinematics kinematics{0.02_s};
+ frc::SE2Odometry odometry{kinematics, frc::Rotation2d{},
+ frc::SE2KinematicPrimitive{frc::Pose2d{}},
+ frc::Pose2d{}};
+ frc::SE2PoseEstimator estimator{
+ odometry, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the buffer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
- estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m);
+ estimator.UpdateWithTime(time, frc::Rotation2d{},
+ frc::SE2KinematicPrimitive{frc::Pose2d{}});
}
auto odometryPose = estimator.GetEstimatedPosition();
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
deleted file mode 100644
index f1024efbc92..00000000000
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ /dev/null
@@ -1,313 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-
-#include "frc/estimator/SwerveDrivePoseEstimator.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-
-void testFollowTrajectory(
- const frc::SwerveDriveKinematics<4>& kinematics,
- frc::SwerveDrivePoseEstimator<4>& estimator,
- const frc::Trajectory& trajectory,
- std::function
- chassisSpeedsGenerator,
- std::function
- visionMeasurementGenerator,
- const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
- const units::second_t dt, const units::second_t kVisionUpdateRate,
- const units::second_t kVisionUpdateDelay, const bool checkError,
- const bool debug) {
- wpi::array positions{wpi::empty_array};
-
- estimator.ResetPosition(frc::Rotation2d{}, positions, startingPose);
-
- std::default_random_engine generator;
- std::normal_distribution distribution(0.0, 1.0);
-
- units::second_t t = 0_s;
-
- std::vector> visionPoses;
- std::vector>
- visionLog;
-
- double maxError = -std::numeric_limits::max();
- double errorSum = 0;
-
- if (debug) {
- fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
- }
-
- while (t < trajectory.TotalTime()) {
- frc::Trajectory::State groundTruthState = trajectory.Sample(t);
-
- // We are due for a new vision measurement if it's been `visionUpdateRate`
- // seconds since the last vision measurement
- if (visionPoses.empty() ||
- visionPoses.back().first + kVisionUpdateRate < t) {
- auto visionPose =
- visionMeasurementGenerator(groundTruthState) +
- frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m},
- frc::Rotation2d{distribution(generator) * 0.05_rad}};
- visionPoses.push_back({t, visionPose});
- }
-
- // We should apply the oldest vision measurement if it has been
- // `visionUpdateDelay` seconds since it was measured
- if (!visionPoses.empty() &&
- visionPoses.front().first + kVisionUpdateDelay < t) {
- auto visionEntry = visionPoses.front();
- estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
- visionPoses.erase(visionPoses.begin());
- visionLog.push_back({t, visionEntry.first, visionEntry.second});
- }
-
- auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
-
- auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
-
- for (size_t i = 0; i < 4; i++) {
- positions[i].distance += moduleStates[i].speed * dt;
- positions[i].angle = moduleStates[i].angle;
- }
-
- auto xhat = estimator.UpdateWithTime(
- t,
- groundTruthState.pose.Rotation() +
- frc::Rotation2d{distribution(generator) * 0.05_rad} -
- trajectory.InitialPose().Rotation(),
- positions);
-
- if (debug) {
- fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
- xhat.Y().value(), xhat.Rotation().Radians().value(),
- groundTruthState.pose.X().value(),
- groundTruthState.pose.Y().value(),
- groundTruthState.pose.Rotation().Radians().value());
- }
-
- double error = groundTruthState.pose.Translation()
- .Distance(xhat.Translation())
- .value();
-
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
- }
-
- if (debug) {
- fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
-
- units::second_t apply_time;
- units::second_t measure_time;
- frc::Pose2d vision_pose;
- for (auto record : visionLog) {
- std::tie(apply_time, measure_time, vision_pose) = record;
- fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
- measure_time.value(), vision_pose.X().value(),
- vision_pose.Y().value(),
- vision_pose.Rotation().Radians().value());
- }
- }
-
- EXPECT_NEAR(endingPose.X().value(),
- estimator.GetEstimatedPosition().X().value(), 0.08);
- EXPECT_NEAR(endingPose.Y().value(),
- estimator.GetEstimatedPosition().Y().value(), 0.08);
- EXPECT_NEAR(endingPose.Rotation().Radians().value(),
- estimator.GetEstimatedPosition().Rotation().Radians().value(),
- 0.15);
-
- if (checkError) {
- // NOLINTNEXTLINE(bugprone-integer-division)
- EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058);
- EXPECT_LT(maxError, 0.2);
- }
-}
-
-TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
- frc::SwerveDriveKinematics<4> kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::SwerveModulePosition fl;
- frc::SwerveModulePosition fr;
- frc::SwerveModulePosition bl;
- frc::SwerveModulePosition br;
-
- frc::SwerveDrivePoseEstimator<4> estimator{
- kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
- frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
-
- frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 135_deg},
- frc::Pose2d{-3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 45_deg}},
- frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
-
- testFollowTrajectory(
- kinematics, estimator, trajectory,
- [&](frc::Trajectory::State& state) {
- return frc::ChassisSpeeds{state.velocity, 0_mps,
- state.velocity * state.curvature};
- },
- [&](frc::Trajectory::State& state) { return state.pose; },
- {0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}},
- 0.02_s, 0.1_s, 0.25_s, true, false);
-}
-
-TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
- frc::SwerveDriveKinematics<4> kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::SwerveModulePosition fl;
- frc::SwerveModulePosition fr;
- frc::SwerveModulePosition bl;
- frc::SwerveModulePosition br;
-
- frc::SwerveDrivePoseEstimator<4> estimator{
- kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
- frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
-
- frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
- std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 135_deg},
- frc::Pose2d{-3_m, 0_m, -90_deg},
- frc::Pose2d{0_m, 0_m, 45_deg}},
- frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
-
- for (units::degree_t offset_direction_degs = 0_deg;
- offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
- for (units::degree_t offset_heading_degs = 0_deg;
- offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
- auto pose_offset = frc::Rotation2d{offset_direction_degs};
- auto heading_offset = frc::Rotation2d{offset_heading_degs};
-
- auto initial_pose =
- trajectory.InitialPose() +
- frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
- pose_offset.Sin() * 1_m},
- heading_offset};
-
- testFollowTrajectory(
- kinematics, estimator, trajectory,
- [&](frc::Trajectory::State& state) {
- return frc::ChassisSpeeds{state.velocity, 0_mps,
- state.velocity * state.curvature};
- },
- [&](frc::Trajectory::State& state) { return state.pose; },
- initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
- 0.25_s, false, false);
- }
- }
-}
-
-TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
- // This tests for multiple vision measurements appled at the same time.
- // The expected behavior is that all measurements affect the estimated pose.
- // The alternative result is that only one vision measurement affects the
- // outcome. If that were the case, after 1000 measurements, the estimated
- // pose would converge to that measurement.
- frc::SwerveDriveKinematics<4> kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::SwerveModulePosition fl;
- frc::SwerveModulePosition fr;
- frc::SwerveModulePosition bl;
- frc::SwerveModulePosition br;
-
- frc::SwerveDrivePoseEstimator<4> estimator{
- kinematics, frc::Rotation2d{},
- {fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
- {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
-
- estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br});
-
- for (int i = 0; i < 1000; i++) {
- estimator.AddVisionMeasurement(
- frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
- estimator.AddVisionMeasurement(
- frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
- estimator.AddVisionMeasurement(
- frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
- }
-
- {
- auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
- auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
- auto dtheta = units::math::abs(
- estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
-
- EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
- }
-
- {
- auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
- auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
- auto dtheta = units::math::abs(
- estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
-
- EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
- }
-
- {
- auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
- auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
- auto dtheta = units::math::abs(
- estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
-
- EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
- }
-}
-
-TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
- frc::SwerveDriveKinematics<4> kinematics{
- frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
- frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
-
- frc::SwerveModulePosition fl;
- frc::SwerveModulePosition fr;
- frc::SwerveModulePosition bl;
- frc::SwerveModulePosition br;
-
- frc::SwerveDrivePoseEstimator<4> estimator{
- kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
- frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
-
- // Add enough measurements to fill up the buffer
- for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
- estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
- }
-
- auto odometryPose = estimator.GetEstimatedPosition();
-
- // Apply a vision measurement from 3 seconds ago
- estimator.AddVisionMeasurement(
- frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
- 1_s, {0.1, 0.1, 0.1});
-
- EXPECT_NEAR(odometryPose.X().value(),
- estimator.GetEstimatedPosition().X().value(), 1e-6);
- EXPECT_NEAR(odometryPose.Y().value(),
- estimator.GetEstimatedPosition().Y().value(), 1e-6);
- EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
- estimator.GetEstimatedPosition().Rotation().Radians().value(),
- 1e-6);
-}