(
- Nat.N7(),
- Nat.N7(),
- Nat.N5(),
+ private final SwerveDrivePoseEstimator m_poseEstimator =
+ new SwerveDrivePoseEstimator(
+ m_kinematics,
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
@@ -55,9 +49,7 @@ public class Drivetrain {
m_backRight.getPosition()
},
new Pose2d(),
- m_kinematics,
- VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05),
- VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01),
+ VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
public Drivetrain() {
@@ -89,12 +81,6 @@ public class Drivetrain {
public void updateOdometry() {
m_poseEstimator.update(
m_gyro.getRotation2d(),
- new SwerveModuleState[] {
- m_frontLeft.getState(),
- m_frontRight.getState(),
- m_backLeft.getState(),
- m_backRight.getState()
- },
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md
new file mode 100644
index 0000000000..04cdf94982
--- /dev/null
+++ b/wpimath/algorithms.md
@@ -0,0 +1,88 @@
+# Algorithms
+
+## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
+
+### Derivation
+
+Model is
+
+```
+ dx/dt = Ax + Bu
+ y = Cx + Du
+```
+
+where A = 0, B = 0, C = I, and D = 0.
+
+The optimal cost-to-go is the P that satisfies
+
+```
+ AᵀP + PA − PBR⁻¹BᵀP + Q = 0
+```
+
+Let A = Aᵀ and B = Cᵀ for state observers.
+
+```
+ AP + PAᵀ − PCᵀR⁻¹CP + Q = 0
+```
+
+Let A = 0, C = I.
+
+```
+ −PR⁻¹P + Q = 0
+```
+
+Solve for P. P, Q, and R are all diagonal, so this can be solved element-wise.
+
+```
+ −pr⁻¹p + q = 0
+ −pr⁻¹p = −q
+ pr⁻¹p = q
+ p²r⁻¹ = q
+ p² = qr
+ p = sqrt(qr)
+```
+
+Now solve for the Kalman gain.
+
+```
+ K = PCᵀ(CPCᵀ + R)⁻¹
+ K = P(P + R)⁻¹
+ k = p(p + r)⁻¹
+ k = sqrt(qr)(sqrt(qr) + r)⁻¹
+ k = sqrt(qr)/(sqrt(qr) + r)
+```
+
+Multiply by sqrt(q/r)/sqrt(q/r).
+
+```
+ k = q/(q + r sqrt(q/r))
+ k = q/(q + sqrt(qr²/r))
+ k = q/(q + sqrt(qr))
+```
+
+### Corner cases
+
+For q = 0 and r ≠ 0,
+
+```
+ k = 0/(0 + sqrt(0))
+ k = 0/0
+```
+
+Apply L'Hôpital's rule to k with respect to q.
+
+```
+ k = 1/(1 + r/(2sqrt(qr)))
+ k = 2sqrt(qr)/(2sqrt(qr) + r)
+ k = 2sqrt(0)/(2sqrt(0) + r)
+ k = 0/r
+ k = 0
+```
+
+For q ≠ 0 and r = 0,
+
+```
+ k = q / (q + sqrt(0))
+ k = q / q
+ k = 1
+```
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 b995011de8..5b15760832 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
@@ -4,168 +4,87 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
-import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.util.WPIUtilJNI;
-import java.util.function.BiConsumer;
+import java.util.Map;
+import java.util.Objects;
/**
- * This class wraps an {@link edu.wpi.first.math.estimator.UnscentedKalmanFilter Unscented Kalman
- * Filter} to fuse latency-compensated vision measurements with differential drive encoder
- * measurements. It will correct for noisy vision measurements and encoder drift. It is intended to
- * be an easy drop-in for {@link edu.wpi.first.math.kinematics.DifferentialDriveOdometry}; in fact,
- * if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call
- * {@link DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
+ * This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
+ * latency-compensated vision measurements with differential drive encoder measurements. It is
+ * intended to be a drop-in replacement for {@link DifferentialDriveOdometry}; in fact, if you never
+ * call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call {@link
+ * DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
* DifferentialDriveOdometry.
*
- * {@link DifferentialDrivePoseEstimator#update} should be called every robot loop (if your robot
- * loops are faster than the default of 20 ms then you should change the {@link
- * DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, double, double, Pose2d,
- * Matrix, Matrix, Matrix, double) nominal delta time}.) {@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.
+ *
{@link DifferentialDrivePoseEstimator#update} should be called every robot loop.
*
- *
The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
+ *
{@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.
*
- *
x = [x, y, theta, dist_l, dist_r]ᵀ in the field coordinate system
- * containing x position, y position, heading, left encoder distance, and right encoder distance.
+ *
The state-space system used internally has the following states (x), and outputs (y):
*
- *
u = [v_x, v_y, omega]ᵀ containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
- *
- *
NB: Using velocities make things considerably easier, because it means that teams don't have
- * to worry about getting an accurate model. Basically, we suspect that it's easier for teams to get
- * good encoder data than it is for them to perform system identification well enough to get a good
- * model.
+ *
x = [x, y, theta]ᵀ in the field coordinate system containing x position, y
+ * position, and heading.
*
*
y = [x, y, theta]ᵀ from vision containing x position, y position, and
- * heading; or y = [dist_l, dist_r, theta] containing left encoder position, right
- * encoder position, and gyro heading.
+ * heading.
*/
public class DifferentialDrivePoseEstimator {
- final UnscentedKalmanFilter m_observer; // Package-private to allow for unit testing
- private final BiConsumer, Matrix> m_visionCorrect;
- private final TimeInterpolatableBuffer m_poseBuffer;
+ private final DifferentialDriveKinematics m_kinematics;
+ private final DifferentialDriveOdometry m_odometry;
+ private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
- private final double m_nominalDt; // Seconds
- private double m_prevTimeSeconds = -1.0;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
- private Matrix m_visionContR;
+ private final TimeInterpolatableBuffer m_poseBuffer =
+ TimeInterpolatableBuffer.createBuffer(1.5);
/**
* Constructs a DifferentialDrivePoseEstimator.
*
+ * @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
- * with units in meters and radians.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
- * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
- * is in the form [dist_l, dist_r, theta]ᵀ, with units in meters and radians.
+ * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+ * meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
*/
public DifferentialDrivePoseEstimator(
+ DifferentialDriveKinematics kinematics,
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters,
- Matrix stateStdDevs,
- Matrix localMeasurementStdDevs,
+ Matrix stateStdDevs,
Matrix visionMeasurementStdDevs) {
- this(
- gyroAngle,
- leftDistanceMeters,
- rightDistanceMeters,
- initialPoseMeters,
- stateStdDevs,
- localMeasurementStdDevs,
- visionMeasurementStdDevs,
- 0.02);
- }
+ m_kinematics = kinematics;
+ m_odometry =
+ new DifferentialDriveOdometry(
+ gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters);
- /**
- * Constructs a DifferentialDrivePoseEstimator.
- *
- * @param gyroAngle The current gyro angle.
- * @param leftDistanceMeters The distance traveled by the left encoder.
- * @param rightDistanceMeters The distance traveled by the right encoder.
- * @param initialPoseMeters The starting pose estimate.
- * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
- * with units in meters and radians.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
- * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
- * is in the form [dist_l, dist_r, theta]ᵀ, with units in meters and radians.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
- * @param nominalDtSeconds The time in seconds between each robot loop.
- */
- public DifferentialDrivePoseEstimator(
- Rotation2d gyroAngle,
- double leftDistanceMeters,
- double rightDistanceMeters,
- Pose2d initialPoseMeters,
- Matrix stateStdDevs,
- Matrix localMeasurementStdDevs,
- Matrix visionMeasurementStdDevs,
- double nominalDtSeconds) {
- m_nominalDt = nominalDtSeconds;
-
- m_observer =
- new UnscentedKalmanFilter<>(
- Nat.N5(),
- Nat.N3(),
- this::f,
- (x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)),
- stateStdDevs,
- localMeasurementStdDevs,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2),
- m_nominalDt);
- m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
+ for (int i = 0; i < 3; ++i) {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+ }
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- m_visionCorrect =
- (u, y) ->
- m_observer.correct(
- Nat.N3(),
- u,
- y,
- (x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
- m_visionContR,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2));
-
- m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
- m_observer.setXhat(fillStateVector(initialPoseMeters, leftDistanceMeters, rightDistanceMeters));
}
/**
@@ -178,42 +97,21 @@ public class DifferentialDrivePoseEstimator {
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) {
- m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
- }
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
- private Matrix f(Matrix x, Matrix u) {
- // Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us.
- var theta = x.get(2, 0);
- var toFieldRotation =
- new MatBuilder<>(Nat.N5(), Nat.N5())
- .fill(
- Math.cos(theta),
- -Math.sin(theta),
- 0,
- 0,
- 0,
- Math.sin(theta),
- Math.cos(theta),
- 0,
- 0,
- 0,
- 0,
- 0,
- 1,
- 0,
- 0,
- 0,
- 0,
- 0,
- 1,
- 0,
- 0,
- 0,
- 0,
- 0,
- 1);
- return toFieldRotation.times(
- VecBuilder.fill(u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 0)));
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row) {
+ if (m_q.get(row, 0) == 0.0) {
+ m_visionK.set(row, row, 0.0);
+ } else {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
}
/**
@@ -233,30 +131,22 @@ public class DifferentialDrivePoseEstimator {
double rightPositionMeters,
Pose2d poseMeters) {
// Reset state estimate and error covariance
- m_observer.reset();
+ m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters);
m_poseBuffer.clear();
-
- m_observer.setXhat(fillStateVector(poseMeters, leftPositionMeters, rightPositionMeters));
-
- m_prevTimeSeconds = -1;
-
- m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
- m_previousAngle = poseMeters.getRotation();
}
/**
- * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
- return new Pose2d(
- m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+ return m_odometry.getPoseMeters();
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
- * estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
@@ -271,21 +161,49 @@ public class DifferentialDrivePoseEstimator {
* DifferentialDrivePoseEstimator#updateWithTime} then you must use a timestamp with an epoch
* since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
* Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
- * source in this case.
+ * source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+ // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);
- if (sample.isPresent()) {
- m_visionCorrect.accept(
- new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0),
- StateSpaceUtil.poseTo3dVector(
- getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
+
+ if (sample.isEmpty()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // Step 3: 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.
+ 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().leftMeters,
+ sample.get().rightMeters,
+ sample.get().poseMeters.exp(scaledTwist));
+
+ // Step 6: 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().leftMeters,
+ entry.getValue().rightMeters);
}
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
- * estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
@@ -318,77 +236,127 @@ public class DifferentialDrivePoseEstimator {
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
- * should be called every loop.
+ * Updates the the Kalman Filter using only wheel encoder information. Note that this should be
+ * called every loop.
*
* @param gyroAngle The current gyro angle.
- * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
- * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
- * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+ * @param distanceLeftMeters The total distance travelled by the left wheel in meters.
+ * @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(
- Rotation2d gyroAngle,
- DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
- double distanceLeftMeters,
- double distanceRightMeters) {
+ Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
return updateWithTime(
- WPIUtilJNI.now() * 1.0e-6,
- gyroAngle,
- wheelVelocitiesMetersPerSecond,
- distanceLeftMeters,
- distanceRightMeters);
+ WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
- * should be called every loop.
+ * Updates the the Kalman Filter using only wheel encoder information. Note that this should be
+ * called every loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
- * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
- * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
- * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
- * last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+ * @param distanceLeftMeters The total distance travelled by the left wheel in meters.
+ * @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(
double currentTimeSeconds,
Rotation2d gyroAngle,
- DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
double distanceLeftMeters,
double distanceRightMeters) {
- double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
- m_prevTimeSeconds = currentTimeSeconds;
-
- var angle = gyroAngle.plus(m_gyroOffset);
- // Diff drive forward kinematics:
- // v_c = (v_l + v_r) / 2
- var wheelVels = wheelVelocitiesMetersPerSecond;
- var u =
- VecBuilder.fill(
- (wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2,
- 0,
- angle.minus(m_previousAngle).getRadians() / dt);
- m_previousAngle = angle;
-
- var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians());
- m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
- m_observer.predict(u, dt);
- m_observer.correct(u, localY);
+ m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters);
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(
+ getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters));
return getEstimatedPosition();
}
- private static Matrix fillStateVector(Pose2d pose, double leftDist, double rightDist) {
- return VecBuilder.fill(
- pose.getTranslation().getX(),
- pose.getTranslation().getY(),
- pose.getRotation().getRadians(),
- leftDist,
- rightDist);
+ /**
+ * 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;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The distance traveled by the left encoder.
+ private final double leftMeters;
+
+ // The distance traveled by the right encoder.
+ private final double rightMeters;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param pose The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param leftMeters The distance traveled by the left encoder.
+ * @param rightMeters The distanced traveled by the right encoder.
+ */
+ private InterpolationRecord(
+ Pose2d poseMeters, Rotation2d gyro, double leftMeters, double rightMeters) {
+ this.poseMeters = poseMeters;
+ this.gyroAngle = gyro;
+ this.leftMeters = leftMeters;
+ this.rightMeters = rightMeters;
+ }
+
+ /**
+ * 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 left distance.
+ var left_lerp = MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t);
+
+ // Find the new right distance.
+ var right_lerp = MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t);
+
+ // Find the new gyro angle.
+ var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+ // Create a twist to represent this change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(left_lerp - leftMeters, right_lerp - rightMeters);
+ twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, left_lerp, right_lerp);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof InterpolationRecord)) {
+ return false;
+ }
+ InterpolationRecord record = (InterpolationRecord) obj;
+ return Objects.equals(gyroAngle, record.gyroAngle)
+ && Double.compare(leftMeters, record.leftMeters) == 0
+ && Double.compare(rightMeters, record.rightMeters) == 0
+ && Objects.equals(poseMeters, record.poseMeters);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(gyroAngle, leftMeters, rightMeters, poseMeters);
+ }
}
}
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 07b0abad38..5b9b23cf2f 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
@@ -4,181 +4,83 @@
package edu.wpi.first.math.estimator;
-import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
+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.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
-import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.numbers.N5;
-import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.util.WPIUtilJNI;
-import java.util.function.BiConsumer;
+import java.util.Map;
+import java.util.Objects;
/**
- * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
- * latency-compensated vision measurements with mecanum drive encoder velocity measurements. It will
- * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
- * drop-in for {@link edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
+ * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
+ * vision measurements with mecanum drive encoder distance measurements. It will correct for noisy
+ * measurements and encoder drift. It is intended to be a drop-in replacement for {@link
+ * edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
*
- * {@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
- * faster or slower than the default of 20 ms, then you should change the nominal delta time using
- * the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
- * MecanumDriveWheelPositions, Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ *
{@link MecanumDrivePoseEstimator#update} should be called every robot loop.
*
*
{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
*
- *
The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
+ *
The state-space system used internally has the following states (x) and outputs (y):
*
- *
x = [x, y, theta, s_fl, s_fr, s_rl, s_rr]ᵀ in the field coordinate system
- * containing x position, y position, and heading, followed by the distance driven by the front
- * left, front right, rear left, and rear right wheels.
- *
- *
u = [v_x, v_y, omega, v_fl, v_fr, v_rl, v_rr]ᵀ containing x velocity, y
- * velocity, and angular rate in the field coordinate system, followed by the velocity of the front
- * left, front right, rear left, and rear right wheels.
+ *
x = [x, y, theta]ᵀ in the field coordinate system containing x position, y
+ * position, and heading, followed by the distance driven by the front left, front right, rear left,
+ * and rear right wheels.
*
*
y = [x, y, theta]ᵀ from vision containing x position, y position, and
- * heading; or y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ containing gyro heading,
- * followed by the distance driven by the front left, front right, rear left, and rear right wheels.
+ * heading.
*/
public class MecanumDrivePoseEstimator {
- private final UnscentedKalmanFilter m_observer;
private final MecanumDriveKinematics m_kinematics;
- private final BiConsumer, Matrix> m_visionCorrect;
- private final TimeInterpolatableBuffer m_poseBuffer;
+ private final MecanumDriveOdometry m_odometry;
+ private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
- private final double m_nominalDt; // Seconds
- private double m_prevTimeSeconds = -1.0;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
- private Matrix m_visionContR;
+ private final TimeInterpolatableBuffer m_poseBuffer =
+ TimeInterpolatableBuffer.createBuffer(1.5);
/**
* Constructs a MecanumDrivePoseEstimator.
*
+ * @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting pose estimate.
- * @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
- * s_rr]ᵀ, with units in meters and radians, followed by meters.
- * @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number
- * to trust sensor readings from the gyro less. This matrix is in the form [theta, s_fl, s_fr,
- * s_rl, s_rr], with units in radians, followed by meters.
+ * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+ * meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
*/
public MecanumDrivePoseEstimator(
+ MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters,
- MecanumDriveKinematics kinematics,
- Matrix stateStdDevs,
- Matrix localMeasurementStdDevs,
+ Matrix stateStdDevs,
Matrix visionMeasurementStdDevs) {
- this(
- gyroAngle,
- wheelPositions,
- initialPoseMeters,
- kinematics,
- stateStdDevs,
- localMeasurementStdDevs,
- visionMeasurementStdDevs,
- 0.02);
- }
-
- /**
- * Constructs a MecanumDrivePoseEstimator.
- *
- * @param gyroAngle The current gyro angle.
- * @param wheelPositions The distances driven by each wheel.
- * @param initialPoseMeters The starting pose estimate.
- * @param kinematics A correctly-configured kinematics object for your drivetrain.
- * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
- * s_rr]ᵀ, with units in meters and radians, followed by meters.
- * @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number
- * to trust sensor readings from the gyro less. This matrix is in the form [theta, s_fl, s_fr,
- * s_rl, s_rr], with units in radians, followed by meters.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
- * @param nominalDtSeconds The time in seconds between each robot loop.
- */
- public MecanumDrivePoseEstimator(
- Rotation2d gyroAngle,
- MecanumDriveWheelPositions wheelPositions,
- Pose2d initialPoseMeters,
- MecanumDriveKinematics kinematics,
- Matrix stateStdDevs,
- Matrix localMeasurementStdDevs,
- Matrix visionMeasurementStdDevs,
- double nominalDtSeconds) {
- m_nominalDt = nominalDtSeconds;
-
- m_observer =
- new UnscentedKalmanFilter<>(
- Nat.N7(),
- Nat.N5(),
- (x, u) -> u,
- (x, u) -> x.block(Nat.N5(), Nat.N1(), 2, 0),
- stateStdDevs,
- localMeasurementStdDevs,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleMean(0),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(0),
- AngleStatistics.angleAdd(2),
- m_nominalDt);
m_kinematics = kinematics;
- m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
+ m_odometry = new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
+
+ for (int i = 0; i < 3; ++i) {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+ }
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- m_visionCorrect =
- (u, y) ->
- m_observer.correct(
- Nat.N3(),
- u,
- y,
- (x, u1) -> x.block(Nat.N3(), Nat.N1(), 0, 0),
- m_visionContR,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2));
-
- m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
-
- var poseVec = StateSpaceUtil.poseTo3dVector(initialPoseMeters);
- var xhat =
- VecBuilder.fill(
- poseVec.get(0, 0),
- poseVec.get(1, 0),
- poseVec.get(2, 0),
- wheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters);
-
- m_observer.setXhat(xhat);
}
/**
@@ -191,7 +93,21 @@ public class MecanumDrivePoseEstimator {
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) {
- m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row) {
+ if (m_q.get(row, 0) == 0.0) {
+ m_visionK.set(row, row, 0.0);
+ } else {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
}
/**
@@ -207,41 +123,22 @@ public class MecanumDrivePoseEstimator {
public void resetPosition(
Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
- m_observer.reset();
+ m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
m_poseBuffer.clear();
-
- var poseVec = StateSpaceUtil.poseTo3dVector(poseMeters);
- var xhat =
- VecBuilder.fill(
- poseVec.get(0, 0),
- poseVec.get(1, 0),
- poseVec.get(2, 0),
- wheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters);
-
- m_observer.setXhat(xhat);
-
- m_prevTimeSeconds = -1;
-
- m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
- m_previousAngle = poseMeters.getRotation();
}
/**
- * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
- return new Pose2d(
- m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+ return m_odometry.getPoseMeters();
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
- * estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
@@ -258,18 +155,41 @@ public class MecanumDrivePoseEstimator {
* Timer.getFPGATimestamp as your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+ // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);
- if (sample.isPresent()) {
- m_visionCorrect.accept(
- new MatBuilder<>(Nat.N7(), Nat.N1()).fill(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
- StateSpaceUtil.poseTo3dVector(
- getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
+
+ if (sample.isEmpty()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // Step 3: 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.
+ 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: 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);
}
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
- * estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
@@ -301,71 +221,141 @@ public class MecanumDrivePoseEstimator {
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
- * called every loop, and the correct loop period must be passed into the constructor of this
- * class.
+ * Updates the Kalman Filter using only wheel encoder information. This should be called every
+ * loop.
*
* @param gyroAngle The current gyro angle.
- * @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
- public Pose2d update(
- Rotation2d gyroAngle,
- MecanumDriveWheelSpeeds wheelSpeeds,
- MecanumDriveWheelPositions wheelPositions) {
- return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds, wheelPositions);
+ public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
+ return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelPositions);
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
- * called every loop, and the correct loop period must be passed into the constructor of this
- * class.
+ * Updates the Kalman Filter using only wheel encoder information. This should be called every
+ * loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
- * @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(
- double currentTimeSeconds,
- Rotation2d gyroAngle,
- MecanumDriveWheelSpeeds wheelSpeeds,
- MecanumDriveWheelPositions wheelPositions) {
- double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
- m_prevTimeSeconds = currentTimeSeconds;
+ double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
+ m_odometry.update(gyroAngle, wheelPositions);
- var angle = gyroAngle.plus(m_gyroOffset);
- var omega = angle.minus(m_previousAngle).getRadians() / dt;
-
- var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
- var fieldRelativeVelocities =
- new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
- .rotateBy(angle);
-
- var u =
- VecBuilder.fill(
- fieldRelativeVelocities.getX(),
- fieldRelativeVelocities.getY(),
- omega,
- wheelSpeeds.frontLeftMetersPerSecond,
- wheelSpeeds.frontRightMetersPerSecond,
- wheelSpeeds.rearLeftMetersPerSecond,
- wheelSpeeds.rearRightMetersPerSecond);
- m_previousAngle = angle;
-
- var localY =
- VecBuilder.fill(
- angle.getRadians(),
- wheelPositions.frontLeftMeters,
- wheelPositions.frontRightMeters,
- wheelPositions.rearLeftMeters,
- wheelPositions.rearRightMeters);
- m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
- m_observer.predict(u, dt);
- m_observer.correct(u, localY);
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(
+ getEstimatedPosition(),
+ gyroAngle,
+ new MecanumDriveWheelPositions(
+ wheelPositions.frontLeftMeters,
+ wheelPositions.frontRightMeters,
+ wheelPositions.rearLeftMeters,
+ wheelPositions.rearRightMeters)));
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;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The distances traveled by each wheel encoder.
+ private final MecanumDriveWheelPositions wheelPositions;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param pose The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param wheelPositions The distances traveled by each wheel encoder.
+ */
+ private InterpolationRecord(
+ Pose2d poseMeters, Rotation2d gyro, MecanumDriveWheelPositions 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 wheels_lerp =
+ new MecanumDriveWheelPositions(
+ MathUtil.interpolate(
+ this.wheelPositions.frontLeftMeters,
+ endValue.wheelPositions.frontLeftMeters,
+ t),
+ MathUtil.interpolate(
+ this.wheelPositions.frontRightMeters,
+ endValue.wheelPositions.frontRightMeters,
+ t),
+ MathUtil.interpolate(
+ this.wheelPositions.rearLeftMeters, endValue.wheelPositions.rearLeftMeters, t),
+ MathUtil.interpolate(
+ this.wheelPositions.rearRightMeters,
+ endValue.wheelPositions.rearRightMeters,
+ t));
+
+ // Find the distance travelled between this measurement and the interpolated measurement.
+ var wheels_delta =
+ new MecanumDriveWheelPositions(
+ wheels_lerp.frontLeftMeters - this.wheelPositions.frontLeftMeters,
+ wheels_lerp.frontRightMeters - this.wheelPositions.frontRightMeters,
+ wheels_lerp.rearLeftMeters - this.wheelPositions.rearLeftMeters,
+ wheels_lerp.rearRightMeters - this.wheelPositions.rearRightMeters);
+
+ // Find the new gyro angle.
+ var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+ // Create a twist to represent this change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(wheels_delta);
+ twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, wheels_lerp);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof InterpolationRecord)) {
+ return false;
+ }
+ InterpolationRecord record = (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);
+ }
+ }
}
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 e3bdc4e7de..ed3e02bf42 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
@@ -4,228 +4,83 @@
package edu.wpi.first.math.estimator;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.Num;
-import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
+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.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
-import java.util.function.BiConsumer;
+import java.util.Map;
+import java.util.Objects;
/**
- * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
- * latency-compensated vision measurements with swerve drive encoder velocity measurements. It will
- * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
- * drop-in for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
+ * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is intended to be a
+ * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
*
- * The generic arguments to this class define the size of the state, input and output vectors
- * used in the underlying {@link UnscentedKalmanFilter Unscented Kalman Filter}. {@link Num States}
- * must be equal to the module count + 3. {@link Num Inputs} must be equal to the module count + 3.
- * {@link Num Outputs} must be equal to the module count + 1.
- *
- *
{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If your loops are
- * faster or slower than the default of 20 ms, then you should change the nominal delta time using
- * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Nat, Nat,
- * Nat, Rotation2d, SwerveModulePosition[], Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix,
- * double)}.
+ *
{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
*
*
{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
- * want; if you never call it, then this class will behave mostly like regular encoder odometry.
+ * want; if you never call it, then this class will behave as regular encoder odometry.
*
- *
The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
+ *
The state-space system used internally has the following states (x) and outputs (y):
*
- *
x = [x, y, theta, s_0, ..., s_n]ᵀ in the field coordinate system containing
- * x position, y position, and heading, followed by the distance travelled by each wheel.
- *
- *
u = [v_x, v_y, omega, v_0, ... v_n]ᵀ containing x velocity, y velocity, and
- * angular rate in the field coordinate system, followed by the velocity measured at each wheel.
+ *
x = [x, y, theta]ᵀ in the field coordinate system containing x position, y
+ * position, and heading.
*
*
y = [x, y, theta]ᵀ from vision containing x position, y position, and
- * heading; or y = [theta, s_0, ..., s_n]ᵀ containing gyro heading, followed by
- * the distance travelled by each wheel.
+ * heading.
*/
-public class SwerveDrivePoseEstimator {
- private final UnscentedKalmanFilter m_observer;
+public class SwerveDrivePoseEstimator {
private final SwerveDriveKinematics m_kinematics;
- private final BiConsumer, Matrix> m_visionCorrect;
- private final TimeInterpolatableBuffer m_poseBuffer;
+ private final SwerveDriveOdometry m_odometry;
+ private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ private final int m_numModules;
+ private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
- private final Nat m_states;
- private final Nat m_inputs;
- private final Nat m_outputs;
-
- private final double m_nominalDt; // Seconds
- private double m_prevTimeSeconds = -1.0;
-
- private Rotation2d m_gyroOffset;
- private Rotation2d m_previousAngle;
-
- private Matrix m_visionContR;
+ private final TimeInterpolatableBuffer m_poseBuffer =
+ TimeInterpolatableBuffer.createBuffer(1.5);
/**
* Constructs a SwerveDrivePoseEstimator.
*
- * @param states The size of the state vector.
- * @param inputs The size of the input vector.
- * @param outputs The size of the outputs vector.
- * @param gyroAngle The current gyro angle.
- * @param initialPoseMeters The starting pose estimate.
- * @param modulePositions The current distance measurements and rotations of the swerve modules.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta, s_0, ... s_n]ᵀ, with
- * units in meters and radians, then meters.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
- * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
- * is in the form [theta, s_0, ... s_n], with units in radians followed by meters.
+ * model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+ * meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
*/
public SwerveDrivePoseEstimator(
- Nat states,
- Nat inputs,
- Nat outputs,
+ SwerveDriveKinematics kinematics,
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose2d initialPoseMeters,
- SwerveDriveKinematics kinematics,
- Matrix stateStdDevs,
- Matrix localMeasurementStdDevs,
+ Matrix stateStdDevs,
Matrix visionMeasurementStdDevs) {
- this(
- states,
- inputs,
- outputs,
- gyroAngle,
- modulePositions,
- initialPoseMeters,
- kinematics,
- stateStdDevs,
- localMeasurementStdDevs,
- visionMeasurementStdDevs,
- 0.02);
- }
-
- /**
- * Constructs a SwerveDrivePoseEstimator.
- *
- * @param states The size of the state vector.
- * @param inputs The size of the input vector.
- * @param outputs The size of the outputs vector.
- * @param gyroAngle The current gyro angle.
- * @param modulePositions The current distance measurements and rotations of the swerve modules.
- * @param initialPoseMeters The starting pose estimate.
- * @param kinematics A correctly-configured kinematics object for your drivetrain.
- * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
- * model's state estimates less. This matrix is in the form [x, y, theta, s_0, ... s_n]ᵀ, with
- * units in meters and radians, then meters.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
- * Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
- * is in the form [theta, s_0, ... s_n], with units in radians followed by meters.
- * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
- * numbers to trust global measurements from vision less. This matrix is in the form [x, y,
- * theta]ᵀ, with units in meters and radians.
- * @param nominalDtSeconds The time in seconds between each robot loop.
- */
- public SwerveDrivePoseEstimator(
- Nat states,
- Nat inputs,
- Nat outputs,
- Rotation2d gyroAngle,
- SwerveModulePosition[] modulePositions,
- Pose2d initialPoseMeters,
- SwerveDriveKinematics kinematics,
- Matrix stateStdDevs,
- Matrix localMeasurementStdDevs,
- Matrix visionMeasurementStdDevs,
- double nominalDtSeconds) {
- this.m_states = states;
- this.m_inputs = inputs;
- this.m_outputs = outputs;
-
- if (states.getNum() != modulePositions.length + 3) {
- throw new IllegalArgumentException(
- String.format(
- "Number of states (%s) must be 3 + "
- + "the number of modules provided in constructor (%s).",
- states.getNum(), modulePositions.length));
- }
-
- if (inputs.getNum() != modulePositions.length + 3) {
- throw new IllegalArgumentException(
- String.format(
- "Number of inputs (%s) must be 3 + "
- + "the number of modules provided in constructor (%s).",
- inputs.getNum(), modulePositions.length));
- }
-
- if (outputs.getNum() != modulePositions.length + 1) {
- throw new IllegalArgumentException(
- String.format(
- "Number of outputs (%s) must be 3 + "
- + "the number of modules provided in constructor (%s).",
- outputs.getNum(), modulePositions.length));
- }
-
- m_nominalDt = nominalDtSeconds;
-
- m_observer =
- new UnscentedKalmanFilter<>(
- states,
- outputs,
- (x, u) -> u.block(states.getNum(), 1, 0, 0),
- (x, u) -> x.block(states.getNum() - 2, 1, 2, 0),
- stateStdDevs,
- localMeasurementStdDevs,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleMean(0),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(0),
- AngleStatistics.angleAdd(2),
- m_nominalDt);
m_kinematics = kinematics;
- m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
+ m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters);
- // Initialize vision R
- setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- m_visionCorrect =
- (u, y) ->
- m_observer.correct(
- Nat.N3(),
- u,
- y,
- (x, u1) -> x.block(3, 1, 0, 0),
- m_visionContR,
- AngleStatistics.angleMean(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleResidual(2),
- AngleStatistics.angleAdd(2));
-
- m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
- m_previousAngle = initialPoseMeters.getRotation();
-
- var poseVec = StateSpaceUtil.poseTo3dVector(initialPoseMeters);
- Matrix xhat = new Matrix(states, Nat.N1());
- xhat.set(0, 0, poseVec.get(0, 0));
- xhat.set(1, 0, poseVec.get(1, 0));
- xhat.set(2, 0, poseVec.get(2, 0));
-
- for (int index = 3; index < states.getNum(); index++) {
- xhat.set(index, 0, modulePositions[index - 3].distanceMeters);
+ for (int i = 0; i < 3; ++i) {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
- m_observer.setXhat(xhat);
+ m_numModules = modulePositions.length;
+
+ setVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
@@ -238,7 +93,21 @@ public class SwerveDrivePoseEstimator visionMeasurementStdDevs) {
- m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row) {
+ if (m_q.get(row, 0) == 0.0) {
+ m_visionK.set(row, row, 0.0);
+ } else {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
}
/**
@@ -254,40 +123,22 @@ public class SwerveDrivePoseEstimator xhat = new Matrix(m_states, Nat.N1());
- xhat.set(0, 0, poseVec.get(0, 0));
- xhat.set(1, 0, poseVec.get(1, 0));
- xhat.set(2, 0, poseVec.get(2, 0));
-
- for (int index = 3; index < m_states.getNum(); index++) {
- xhat.set(index, 0, modulePositions[index - 3].distanceMeters);
- }
-
- m_observer.setXhat(xhat);
-
- m_prevTimeSeconds = -1;
-
- m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
- m_previousAngle = poseMeters.getRotation();
}
/**
- * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
- return new Pose2d(
- m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+ return m_odometry.getPoseMeters();
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
- * estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
@@ -304,18 +155,41 @@ public class SwerveDrivePoseEstimator(m_inputs, Nat.N1()),
- StateSpaceUtil.poseTo3dVector(
- getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
+
+ if (sample.isEmpty()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // Step 3: 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.
+ 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().modulePositions,
+ sample.get().poseMeters.exp(scaledTwist));
+
+ // Step 6: 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().modulePositions);
}
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
- * estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+ * while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
@@ -347,70 +221,140 @@ public class SwerveDrivePoseEstimator= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
- m_prevTimeSeconds = currentTimeSeconds;
-
- var angle = gyroAngle.plus(m_gyroOffset);
- var omega = angle.minus(m_previousAngle).getRadians() / dt;
-
- var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates);
- var fieldRelativeVelocities =
- new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
- .rotateBy(angle);
-
- var u = new Matrix(m_inputs, Nat.N1());
-
- u.set(0, 0, fieldRelativeVelocities.getX());
- u.set(1, 0, fieldRelativeVelocities.getY());
- u.set(2, 0, omega);
- for (int index = 3; index < m_inputs.getNum(); index++) {
- u.set(index, 0, moduleStates[index - 3].speedMetersPerSecond);
+ double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+ if (modulePositions.length != m_numModules) {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
}
- m_previousAngle = angle;
+ var internalModulePositions = new SwerveModulePosition[m_numModules];
- var localY = new Matrix(m_outputs, Nat.N1());
- localY.set(0, 0, angle.getRadians());
- for (int index = 1; index < m_outputs.getNum(); index++) {
- localY.set(index, 0, modulePositions[index - 1].distanceMeters);
+ for (int i = 0; i < m_numModules; i++) {
+ internalModulePositions[i] =
+ new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
}
- m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
- m_observer.predict(u, dt);
- m_observer.correct(u, localY);
+ m_odometry.update(gyroAngle, internalModulePositions);
+
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(getEstimatedPosition(), gyroAngle, internalModulePositions));
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;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The distances and rotations measured at each module.
+ private final SwerveModulePosition[] modulePositions;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param pose The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param wheelPositions The distances and rotations measured at each wheel.
+ */
+ private InterpolationRecord(
+ Pose2d poseMeters, Rotation2d gyro, SwerveModulePosition[] modulePositions) {
+ this.poseMeters = poseMeters;
+ this.gyroAngle = gyro;
+ this.modulePositions = modulePositions;
+ }
+
+ /**
+ * 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 modulePositions = new SwerveModulePosition[m_numModules];
+
+ // Find the distance travelled between this measurement and the interpolated measurement.
+ var moduleDeltas = new SwerveModulePosition[m_numModules];
+
+ for (int i = 0; i < m_numModules; i++) {
+ double ds =
+ MathUtil.interpolate(
+ this.modulePositions[i].distanceMeters,
+ endValue.modulePositions[i].distanceMeters,
+ t);
+ Rotation2d theta =
+ this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
+ modulePositions[i] = new SwerveModulePosition(ds, theta);
+ moduleDeltas[i] =
+ new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
+ }
+
+ // Find the new gyro angle.
+ var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+ // Create a twist to represent this change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
+ twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, modulePositions);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (this == obj) {
+ return true;
+ }
+ if (!(obj instanceof InterpolationRecord)) {
+ return false;
+ }
+ InterpolationRecord record = (InterpolationRecord) obj;
+ return Objects.equals(gyroAngle, record.gyroAngle)
+ && Objects.equals(modulePositions, record.modulePositions)
+ && Objects.equals(poseMeters, record.poseMeters);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(gyroAngle, modulePositions, poseMeters);
+ }
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
index 013dce02b0..7e0712d42a 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
@@ -134,6 +134,16 @@ public final class TimeInterpolatableBuffer {
}
}
+ /**
+ * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
+ * stored within this buffer.
+ *
+ * @return The internal sample buffer.
+ */
+ public NavigableMap getInternalBuffer() {
+ return m_pastSnapshots;
+ }
+
public interface InterpolateFunction {
/**
* Return the interpolated value. This object is assumed to be the starting position, or lower
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
index 0a5aeaf52b..0dfb016306 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -6,6 +6,7 @@ package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Twist2d;
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
@@ -57,4 +58,20 @@ public class DifferentialDriveKinematics {
chassisSpeeds.vxMetersPerSecond
+ trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
}
+
+ /**
+ * Performs forward kinematics to return the resulting Twist2d from the given left and right side
+ * distance deltas. This method is often used for odometry -- determining the robot's position on
+ * the field using changes in the distance driven by each wheel on the robot.
+ *
+ * @param leftDistanceMeters The distance measured by the left side encoder.
+ * @param rightDistanceMeters The distance measured by the right side encoder.
+ * @return The resulting Twist2d.
+ */
+ public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
+ return new Twist2d(
+ (leftDistanceMeters + rightDistanceMeters) / 2,
+ 0,
+ (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
+ }
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
index 3bf92ba08d..c2e188f23c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
@@ -134,6 +134,7 @@ public class SwerveDriveOdometry {
moduleDeltas[index] =
new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
+ previous.distanceMeters = current.distanceMeters;
}
var angle = gyroAngle.plus(m_gyroOffset);
@@ -145,11 +146,7 @@ public class SwerveDriveOdometry {
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
- for (int index = 0; index < m_numModules; index++) {
- m_previousModulePositions[index] =
- new SwerveModulePosition(
- modulePositions[index].distanceMeters, modulePositions[index].angle);
- }
+
return m_poseMeters;
}
}
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 25f56eaad1..dde169ba69 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -11,44 +11,64 @@
using namespace frc;
+DifferentialDrivePoseEstimator::InterpolationRecord
+DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate(
+ DifferentialDriveKinematics& kinematics, InterpolationRecord endValue,
+ double i) const {
+ if (i < 0) {
+ return *this;
+ } else if (i > 1) {
+ return endValue;
+ } else {
+ // Find the interpolated left distance.
+ auto left = wpi::Lerp(this->leftDistance, endValue.leftDistance, i);
+ // Find the interpolated right distance.
+ auto right = wpi::Lerp(this->rightDistance, endValue.rightDistance, i);
+
+ // Find the new gyro angle.
+ auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+ // Create a twist to represent this changed based on the interpolated
+ // sensor inputs.
+ auto twist =
+ kinematics.ToTwist2d(left - leftDistance, right - rightDistance);
+ twist.dtheta = (gyro - gyroAngle).Radians();
+
+ return {pose.Exp(twist), gyro, left, right};
+ }
+}
+
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
- const Rotation2d& gyroAngle, units::meter_t leftDistance,
- units::meter_t rightDistance, const Pose2d& initialPose,
- const wpi::array& stateStdDevs,
- const wpi::array& localMeasurementStdDevs,
- const wpi::array& visionMeasurmentStdDevs,
- units::second_t nominalDt)
- : m_observer(
- &DifferentialDrivePoseEstimator::F,
- [](const Vectord<5>& x, const Vectord<3>& u) {
- return Vectord<3>{x(3, 0), x(4, 0), x(2, 0)};
- },
- stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
- frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
- frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt),
- m_nominalDt(nominalDt) {
- SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
+ DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ units::meter_t leftDistance, units::meter_t rightDistance,
+ const Pose2d& initialPose, const wpi::array& stateStdDevs,
+ const wpi::array& visionMeasurementStdDevs)
+ : m_kinematics{kinematics},
+ m_odometry{gyroAngle, leftDistance, rightDistance, initialPose} {
+ for (size_t i = 0; i < 3; ++i) {
+ m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+ }
- // Create correction mechanism for vision measurements.
- m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
- m_observer.Correct<3>(
- u, y,
- [](const Vectord<5>& x, const Vectord<3>&) {
- return x.block<3, 1>(0, 0);
- },
- m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
- frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
- };
-
- m_gyroOffset = initialPose.Rotation() - gyroAngle;
- m_previousAngle = initialPose.Rotation();
- m_observer.SetXhat(FillStateVector(initialPose, leftDistance, rightDistance));
+ SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
- const wpi::array& visionMeasurmentStdDevs) {
- // Create R (covariances) for vision measurements.
- m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+ const wpi::array& visionMeasurementStdDevs) {
+ wpi::array r{wpi::empty_array};
+ for (size_t i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (size_t row = 0; row < 3; ++row) {
+ if (m_q[row] == 0.0) {
+ m_visionK(row, row) = 0.0;
+ } else {
+ m_visionK(row, row) =
+ m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+ }
+ }
}
void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
@@ -56,85 +76,81 @@ void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
units::meter_t rightDistance,
const Pose2d& pose) {
// Reset state estimate and error covariance
- m_observer.Reset();
+ m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose);
m_poseBuffer.Clear();
-
- m_observer.SetXhat(FillStateVector(pose, leftDistance, rightDistance));
-
- m_prevTime = -1_s;
-
- m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
- m_previousAngle = pose.Rotation();
}
Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
- return Pose2d{units::meter_t{m_observer.Xhat(0)},
- units::meter_t{m_observer.Xhat(1)},
- units::radian_t{m_observer.Xhat(2)}};
+ return m_odometry.GetPose();
}
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
- if (auto sample = m_poseBuffer.Sample(timestamp)) {
- m_visionCorrect(Vectord<3>::Zero(),
- PoseTo3dVector(GetEstimatedPosition().TransformBy(
- visionRobotPose - sample.value())));
+ // Step 1: Get the estimated pose from when the vision measurement was made.
+ auto sample = m_poseBuffer.Sample(timestamp);
+
+ if (!sample.has_value()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ auto twist = sample.value().pose.Log(visionRobotPose);
+
+ // Step 3: 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.
+ frc::Vectord<3> k_times_twist =
+ m_visionK *
+ frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
+
+ // Step 4: 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().leftDistance,
+ sample.value().rightDistance, sample.value().pose.Exp(scaledTwist));
+
+ // Step 6: 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 first_newer_record =
+ std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
+ [](const auto& pair, auto t) { return t > pair.first; });
+
+ for (auto entry = first_newer_record + 1; entry != internal_buf.end();
+ entry++) {
+ UpdateWithTime(entry->first, entry->second.gyroAngle,
+ entry->second.leftDistance, entry->second.rightDistance);
}
}
-Pose2d DifferentialDrivePoseEstimator::Update(
- const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
- units::meter_t leftDistance, units::meter_t rightDistance) {
+Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- wheelSpeeds, leftDistance, rightDistance);
+ leftDistance, rightDistance);
}
Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
units::meter_t leftDistance, units::meter_t rightDistance) {
- auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
- m_prevTime = currentTime;
+ m_odometry.Update(gyroAngle, leftDistance, rightDistance);
- auto angle = gyroAngle + m_gyroOffset;
- auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
+ // fmt::print("odo, {}, {}, {}, {}, {}, {}\n",
+ // gyroAngle.Radians(),
+ // leftDistance,
+ // rightDistance,
+ // GetEstimatedPosition().X(),
+ // GetEstimatedPosition().Y(),
+ // GetEstimatedPosition().Rotation().Radians()
+ // );
- auto u = Vectord<3>{(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0,
- omega.value()};
-
- m_previousAngle = angle;
-
- auto localY = Vectord<3>{leftDistance.value(), rightDistance.value(),
- angle.Radians().value()};
-
- m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
- m_observer.Predict(u, dt);
- m_observer.Correct(u, localY);
+ m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
+ leftDistance, rightDistance});
return GetEstimatedPosition();
}
-
-Vectord<5> DifferentialDrivePoseEstimator::F(const Vectord<5>& x,
- const Vectord<3>& u) {
- // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
- // that for us.
- auto& theta = x(2);
- Matrixd<5, 5> toFieldRotation{
- {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
- {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
- {0.0, 0.0, 1.0, 0.0, 0.0},
- {0.0, 0.0, 0.0, 1.0, 0.0},
- {0.0, 0.0, 0.0, 0.0, 1.0}};
- return toFieldRotation *
- Vectord<5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
-}
-
-Vectord<5> DifferentialDrivePoseEstimator::FillStateVector(
- const Pose2d& pose, units::meter_t leftDistance,
- units::meter_t rightDistance) {
- return Vectord<5>{pose.Translation().X().value(),
- pose.Translation().Y().value(),
- pose.Rotation().Radians().value(), leftDistance.value(),
- rightDistance.value()};
-}
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index fa07073805..f381435d71 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -11,141 +11,152 @@
using namespace frc;
+frc::MecanumDrivePoseEstimator::InterpolationRecord
+frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate(
+ MecanumDriveKinematics& kinematics, InterpolationRecord endValue,
+ double i) const {
+ if (i < 0) {
+ return *this;
+ } else if (i > 1) {
+ return endValue;
+ } else {
+ // Find the new wheel distance measurements.
+ MecanumDriveWheelPositions wheels_lerp{
+ wpi::Lerp(this->wheelPositions.frontLeft,
+ endValue.wheelPositions.frontLeft, i),
+ wpi::Lerp(this->wheelPositions.frontRight,
+ endValue.wheelPositions.frontRight, i),
+ wpi::Lerp(this->wheelPositions.rearLeft,
+ endValue.wheelPositions.rearLeft, i),
+ wpi::Lerp(this->wheelPositions.rearRight,
+ endValue.wheelPositions.rearRight, i)};
+
+ // Find the distance between this measurement and the
+ // interpolated measurement.
+ MecanumDriveWheelPositions wheels_delta{
+ wheels_lerp.frontLeft - this->wheelPositions.frontLeft,
+ wheels_lerp.frontRight - this->wheelPositions.frontRight,
+ wheels_lerp.rearLeft - this->wheelPositions.rearLeft,
+ wheels_lerp.rearRight - this->wheelPositions.rearRight};
+
+ // Find the new gyro angle.
+ auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+ // Create a twist to represent this changed based on the interpolated
+ // sensor inputs.
+ auto twist = kinematics.ToTwist2d(wheels_delta);
+ twist.dtheta = (gyro - gyroAngle).Radians();
+
+ return {pose.Exp(twist), gyro, wheels_lerp};
+ }
+}
+
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
- const Rotation2d& gyroAngle,
+ MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
- MecanumDriveKinematics kinematics,
- const wpi::array& stateStdDevs,
- const wpi::array& localMeasurementStdDevs,
- const wpi::array& visionMeasurementStdDevs,
- units::second_t nominalDt)
- : m_observer([](const Vectord<7>& x, const Vectord<7>& u) { return u; },
- [](const Vectord<7>& x, const Vectord<7>& u) {
- return x.block<5, 1>(2, 0);
- },
- stateStdDevs, localMeasurementStdDevs, frc::AngleMean<7, 7>(2),
- frc::AngleMean<5, 7>(0), frc::AngleResidual<7>(2),
- frc::AngleResidual<5>(0), frc::AngleAdd<7>(2), nominalDt),
- m_kinematics(kinematics),
- m_nominalDt(nominalDt) {
+ const wpi::array& stateStdDevs,
+ const wpi::array& visionMeasurementStdDevs)
+ : m_kinematics{kinematics},
+ m_odometry{kinematics, gyroAngle, wheelPositions, initialPose} {
+ for (size_t i = 0; i < 3; ++i) {
+ m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+ }
+
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- // Create vision correction mechanism.
- m_visionCorrect = [&](const Vectord<7>& u, const Vectord<3>& y) {
- m_observer.Correct<3>(
- u, y,
- [](const Vectord<7>& x, const Vectord<7>& u) {
- return x.template block<3, 1>(0, 0);
- },
- m_visionContR, frc::AngleMean<3, 7>(2), frc::AngleResidual<3>(2),
- frc::AngleResidual<7>(2), frc::AngleAdd<7>(2));
- };
-
- // Set initial state.
- auto poseVec = PoseTo3dVector(initialPose);
- auto xhat = Vectord<7>{
- poseVec(0),
- poseVec(1),
- poseVec(2),
- wheelPositions.frontLeft.value(),
- wheelPositions.frontRight.value(),
- wheelPositions.rearLeft.value(),
- wheelPositions.rearRight.value(),
- };
-
- m_observer.SetXhat(xhat);
-
- // Calculate offsets.
- m_gyroOffset = initialPose.Rotation() - gyroAngle;
- m_previousAngle = initialPose.Rotation();
}
void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
- const wpi::array& visionMeasurmentStdDevs) {
- // Create R (covariances) for vision measurements.
- m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+ const wpi::array& visionMeasurementStdDevs) {
+ wpi::array r{wpi::empty_array};
+ for (size_t i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (size_t row = 0; row < 3; ++row) {
+ if (m_q[row] == 0.0) {
+ m_visionK(row, row) = 0.0;
+ } else {
+ m_visionK(row, row) =
+ m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+ }
+ }
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
- m_observer.Reset();
+ m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_poseBuffer.Clear();
-
- auto poseVec = PoseTo3dVector(pose);
- auto xhat = Vectord<7>{
- poseVec(0),
- poseVec(1),
- poseVec(2),
- wheelPositions.frontLeft.value(),
- wheelPositions.frontRight.value(),
- wheelPositions.rearLeft.value(),
- wheelPositions.rearRight.value(),
- };
-
- m_observer.SetXhat(xhat);
-
- m_prevTime = -1_s;
-
- m_gyroOffset = pose.Rotation() - gyroAngle;
- m_previousAngle = pose.Rotation();
}
Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
- return Pose2d{m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
- units::radian_t{m_observer.Xhat(2)}};
+ return m_odometry.GetPose();
}
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
- if (auto sample = m_poseBuffer.Sample(timestamp)) {
- m_visionCorrect(Vectord<7>::Zero(),
- PoseTo3dVector(GetEstimatedPosition().TransformBy(
- visionRobotPose - sample.value())));
+ // Step 1: Get the estimated pose from when the vision measurement was made.
+ auto sample = m_poseBuffer.Sample(timestamp);
+
+ if (!sample.has_value()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose
+ auto twist = sample.value().pose.Log(visionRobotPose);
+
+ // Step 3: 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.
+ frc::Vectord<3> k_times_twist =
+ m_visionK *
+ frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
+
+ // Step 4: 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: 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);
}
}
Pose2d frc::MecanumDrivePoseEstimator::Update(
- const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds,
+ const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- wheelSpeeds, wheelPositions);
+ wheelPositions);
}
Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
- const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions) {
- auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
- m_prevTime = currentTime;
+ m_odometry.Update(gyroAngle, wheelPositions);
- auto angle = gyroAngle + m_gyroOffset;
- auto omega = (angle - m_previousAngle).Radians() / dt;
+ MecanumDriveWheelPositions internalWheelPositions{
+ wheelPositions.frontLeft, wheelPositions.frontRight,
+ wheelPositions.rearLeft, wheelPositions.rearRight};
- auto chassisSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
- auto fieldRelativeVelocities =
- Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy(
- angle);
-
- Vectord<7> u{fieldRelativeVelocities.X().value(),
- fieldRelativeVelocities.Y().value(),
- omega.value(),
- wheelSpeeds.frontLeft.value(),
- wheelSpeeds.frontRight.value(),
- wheelSpeeds.rearLeft.value(),
- wheelSpeeds.rearRight.value()};
-
- Vectord<5> localY{angle.Radians().value(), wheelPositions.frontLeft.value(),
- wheelPositions.frontRight.value(),
- wheelPositions.rearLeft.value(),
- wheelPositions.rearRight.value()};
- m_previousAngle = angle;
-
- m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
-
- m_observer.Predict(u, dt);
- m_observer.Correct(u, localY);
+ m_poseBuffer.AddSample(
+ currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
return GetEstimatedPosition();
}
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index c66f1c67b3..d8502bf6a3 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -12,12 +12,14 @@
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
/**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * This class wraps Differential Drive Odometry to fuse latency-compensated
* vision measurements with differential drive encoder measurements. It will
* correct for noisy vision measurements and encoder drift. It is intended to be
* an easy drop-in for DifferentialDriveOdometry. In fact, if you never call
@@ -31,30 +33,22 @@ namespace frc {
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
*
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
+ * The state-space system used internally has the following states (x) and
+ * outputs (y):
*
- * x = [x, y, theta, dist_l, dist_r]ᵀ in the field coordinate
- * system containing x position, y position, heading, left encoder distance,
- * and right encoder distance.
- *
- * u = [v_x, v_y, omega]ᵀ containing x velocity, y velocity,
- * and angular velocity in the field coordinate system.
- *
- * NB: Using velocities make things considerably easier, because it means that
- * teams don't have to worry about getting an accurate model. Basically, we
- * suspect that it's easier for teams to get good encoder data than it is for
- * them to perform system identification well enough to get a good model.
+ * x = [x, y, theta]ᵀ in the field coordinate
+ * system containing x position, y position, and heading.
*
* y = [x, y, theta]ᵀ from vision containing x position, y
- * position, and heading; or y = [dist_l, dist_r, theta]
- * containing left encoder position, right encoder position, and gyro heading.
+ * position, and heading.
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
public:
/**
* Constructs a DifferentialDrivePoseEstimator.
*
+ * @param kinematics A correctly-configured kinematics object
+ * for your drivetrain.
* @param gyroAngle The gyro angle of the robot.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
@@ -65,28 +59,18 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* is in the form
* [x, y, theta, dist_l, dist_r]ᵀ,
* with units in meters and radians.
- * @param localMeasurementStdDevs Standard deviations of the encoder and gyro
- * measurements. Increase these numbers to
- * trust sensor readings from
- * encoders and gyros less.
- * This matrix is in the form
- * [dist_l, dist_r, theta]ᵀ, with units in
- * meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from
* vision less. This matrix is in the form
* [x, y, theta]ᵀ, with units in meters and
* radians.
- * @param nominalDt The period of the loop calling Update().
*/
DifferentialDrivePoseEstimator(
- const Rotation2d& gyroAngle, units::meter_t leftDistance,
- units::meter_t rightDistance, const Pose2d& initialPose,
- const wpi::array& stateStdDevs,
- const wpi::array& localMeasurementStdDevs,
- const wpi::array& visionMeasurementStdDevs,
- units::second_t nominalDt = 20_ms);
+ DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+ units::meter_t leftDistance, units::meter_t rightDistance,
+ const Pose2d& initialPose, const wpi::array& stateStdDevs,
+ const wpi::array& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust of global measurements. This might be used
@@ -106,11 +90,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
- * IF leftDistance and rightDistance are unspecified,
- * You NEED to reset your encoders (to zero). The
- * gyroscope angle does not need to be reset here on the user's robot code.
- * The library automatically takes care of offsetting the gyro angle.
- *
* @param gyroAngle The current gyro angle.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
@@ -120,15 +99,14 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
units::meter_t rightDistance, const Pose2d& pose);
/**
- * Returns the pose of the robot at the current time as estimated by the
- * Unscented Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose.
*/
Pose2d GetEstimatedPosition() const;
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+ * Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
@@ -153,7 +131,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
units::second_t timestamp);
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+ * Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
@@ -199,15 +177,13 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* Note that this should be called every loop iteration.
*
* @param gyroAngle The current gyro angle.
- * @param wheelSpeeds The velocities of the wheels in meters per second.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*
* @return The estimated pose of the robot.
*/
- Pose2d Update(const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
- units::meter_t leftDistance, units::meter_t rightDistance);
+ Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance);
/**
* Updates the Unscented Kalman Filter using only wheel encoder information.
@@ -215,7 +191,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
*
* @param currentTime The time at which this method was called.
* @param gyroAngle The current gyro angle.
- * @param wheelSpeeds The velocities of the wheels in meters per second.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*
@@ -223,27 +198,62 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds,
units::meter_t leftDistance,
units::meter_t rightDistance);
private:
- UnscentedKalmanFilter<5, 3, 3> m_observer;
- TimeInterpolatableBuffer m_poseBuffer{1.5_s};
- std::function& u, const Vectord<3>& y)> m_visionCorrect;
+ struct InterpolationRecord {
+ // The pose observed given the current sensor inputs and the previous pose.
+ Pose2d pose;
- Matrixd<3, 3> m_visionContR;
+ // The current gyro angle.
+ Rotation2d gyroAngle;
- units::second_t m_nominalDt;
- units::second_t m_prevTime = -1_s;
+ // The distance traveled by the left encoder.
+ units::meter_t leftDistance;
- Rotation2d m_gyroOffset;
- Rotation2d m_previousAngle;
+ // The distance traveled by the right encoder.
+ units::meter_t rightDistance;
- static Vectord<5> F(const Vectord<5>& x, const Vectord<3>& u);
- static Vectord<5> FillStateVector(const Pose2d& pose,
- units::meter_t leftDistance,
- units::meter_t rightDistance);
+ /**
+ * 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(DifferentialDriveKinematics& kinematics,
+ InterpolationRecord endValue,
+ double i) const;
+ };
+
+ DifferentialDriveKinematics& m_kinematics;
+ DifferentialDriveOdometry m_odometry;
+ wpi::array m_q{wpi::empty_array};
+ Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+ TimeInterpolatableBuffer m_poseBuffer{
+ 1.5_s, [this](const InterpolationRecord& start,
+ const InterpolationRecord& end, double t) {
+ return start.Interpolate(this->m_kinematics, end, t);
+ }};
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index 2bcab513d7..4d42782bda 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -15,14 +15,15 @@
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry.h"
#include "units/time.h"
namespace frc {
/**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * This class wraps Mecanum Drive Odometry to fuse latency-compensated
* vision measurements with mecanum drive encoder velocity measurements. It will
* correct for noisy measurements and encoder drift. It is intended to be an
- * easy but more accurate drop-in for MecanumDriveOdometry.
+ * easy drop-in for MecanumDriveOdometry.
*
* Update() should be called every robot loop. If your loops are faster or
* slower than the default of 20 ms, then you should change the nominal delta
@@ -32,63 +33,43 @@ namespace frc {
* never call it, then this class will behave mostly like regular encoder
* odometry.
*
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
+ * The state-space system used internally has the following states (x) and
+ * outputs (y):
*
- * x = [x, y, theta, s_fl, s_fr, s_rl, s_rr]ᵀ in the field
- * coordinate system containing x position, y position, and heading, followed
- * by the distance driven by the front left, front right, rear left, and rear
- * right wheels.
- *
- * u = [v_x, v_y, omega, v_fl, v_fr, v_rl, v_rr]ᵀ containing
- * x velocity, y velocity, and angular rate in the field coordinate system,
- * followed by the velocity of the front left, front right, rear left, and rear
- * right wheels.
+ * x = [x, y, theta]ᵀ in the field
+ * coordinate system containing x position, y position, and heading.
*
* y = [x, y, theta]ᵀ from vision containing x position, y
- * position, and heading; or y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ
- * containing gyro heading, followed by the distance driven by the
- * front left, front right, rear left, and rear right wheels.
+ * position, and heading.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
public:
/**
* Constructs a MecanumDrivePoseEstimator.
*
+ * @param kinematics A correctly-configured kinematics object
+ * for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distance measured by each wheel.
* @param initialPose The starting pose estimate.
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
+
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
- * is in the form [x, y, theta, s_fl, s_fr,
- * s_rl, s_rr]ᵀ, with units in meters and
- * radians, followed by meters.
- * @param localMeasurementStdDevs Standard deviation of the gyro
- * measurement. Increase this number to trust
- * sensor readings from the gyro less. This
- * matrix is in the form [theta, s_fl, s_fr,
- * s_rl, s_rr], with units in radians,
- * followed by meters.
+ * is in the form [x, y, theta]ᵀ, with units
+ * in meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]ᵀ, with units in meters and
* radians.
- * @param nominalDt The time in seconds between each robot
- * loop.
*/
MecanumDrivePoseEstimator(
- const Rotation2d& gyroAngle,
+ MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
- const Pose2d& initialPose, MecanumDriveKinematics kinematics,
- const wpi::array& stateStdDevs,
- const wpi::array& localMeasurementStdDevs,
- const wpi::array& visionMeasurementStdDevs,
- units::second_t nominalDt = 20_ms);
+ const Pose2d& initialPose, const wpi::array& stateStdDevs,
+ const wpi::array& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust of global measurements. This might be used
@@ -108,9 +89,6 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
- * IF wheelPositions are unspecified,
- * You NEED to reset your encoders (to zero).
- *
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
@@ -123,15 +101,14 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
const Pose2d& pose);
/**
- * Gets the pose of the robot at the current time as estimated by the Extended
- * Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct
+ * Add a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
@@ -156,7 +133,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
units::second_t timestamp);
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+ * Adds a vision measurement to the Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
@@ -198,48 +175,79 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the the Kalman Filter using only wheel encoder
+ * information. This should be called every loop.
*
* @param gyroAngle The current gyro angle.
- * @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
- const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the the Kalman Filter using only wheel encoder
+ * information. This should be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
- * @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
- const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
private:
- UnscentedKalmanFilter<7, 7, 5> m_observer;
- MecanumDriveKinematics m_kinematics;
- TimeInterpolatableBuffer m_poseBuffer{1.5_s};
- std::function& u, const Vectord<3>& y)> m_visionCorrect;
+ struct InterpolationRecord {
+ // The pose observed given the current sensor inputs and the previous pose.
+ Pose2d pose;
- Eigen::Matrix3d m_visionContR;
+ // The current gyroscope angle.
+ Rotation2d gyroAngle;
- units::second_t m_nominalDt;
- units::second_t m_prevTime = -1_s;
+ // The distances measured at each wheel.
+ MecanumDriveWheelPositions wheelPositions;
- Rotation2d m_gyroOffset;
- Rotation2d m_previousAngle;
+ /**
+ * 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(MecanumDriveKinematics& kinematics,
+ InterpolationRecord endValue,
+ double i) const;
+ };
+
+ MecanumDriveKinematics& m_kinematics;
+ MecanumDriveOdometry m_odometry;
+ wpi::array m_q{wpi::empty_array};
+ Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+ TimeInterpolatableBuffer m_poseBuffer{
+ 1.5_s, [this](const InterpolationRecord& start,
+ const InterpolationRecord& end, double t) {
+ return start.Interpolate(this->m_kinematics, end, t);
+ }};
};
} // 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 9291374a25..fcf2187e16 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -4,143 +4,85 @@
#pragma once
-#include
+#include
+#include
#include
#include
#include
#include "frc/EigenCore.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/estimator/AngleStatistics.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
#include "units/time.h"
namespace frc {
+
/**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
- * vision measurements with swerve drive encoder velocity measurements. It will
- * correct for noisy measurements and encoder drift. It is intended to be an
- * easy but more accurate drop-in for SwerveDriveOdometry.
+ * This class wraps Swerve Drive Odometry to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is
+ * intended to be a drop-in for SwerveDriveOdometry.
*
- * Update() should be called every robot loop. If your loops are faster or
- * slower than the default of 20 ms, then you should change the nominal delta
- * time by specifying it in the constructor.
+ * Update() should be called every robot loop.
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
- * never call it, then this class will behave mostly like regular encoder
+ * never call it, then this class will behave as regular encoder
* odometry.
*
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
+ * The state-space system used internally has the following states (x) and
+ * outputs (y):
*
- * x = [x, y, theta, s_0, ..., s_n]ᵀ in the field coordinate
- * system containing x position, y position, and heading, followed by the
- * distance travelled by each wheel.
- *
- * u = [v_x, v_y, omega, v_0, ... v_n]ᵀ containing x
- * velocity, y velocity, and angular velocity in the field coordinate system,
- * followed by the velocity measured at each wheel.
+ * x = [x, y, theta]ᵀ in the field coordinate
+ * system containing x position, y position, and heading.
*
* y = [x, y, theta]ᵀ from vision containing x position, y
- * position, and heading; or y = [theta, s_0, ..., s_n]ᵀ
- * containing gyro heading, followed by the distance travelled by each wheel.
+ * position, and heading.
*/
template
class SwerveDrivePoseEstimator {
public:
- static constexpr size_t States = 3 + NumModules;
- static constexpr size_t Inputs = 3 + NumModules;
- static constexpr size_t Outputs = 1 + NumModules;
-
/**
* Constructs a SwerveDrivePoseEstimator.
*
+ * @param kinematics A correctly-configured kinematics object
+ * for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance and rotation
* measurements of the swerve modules.
* @param initialPose The starting pose estimate.
- * @param kinematics A correctly-configured kinematics object
- * for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
- * is in the form [x, y, theta, s_0, ...
- * s_n]ᵀ, with units in meters and radians, then meters.
- * @param localMeasurementStdDevs Standard deviation of the gyro measurement.
- * Increase this number to trust sensor
- * readings from the gyro less. This matrix is
- * in the form [theta, s_0, ... s_n], with
- * units in radians followed by meters.
+ * is in the form [x, y, theta]ᵀ, with units
+ * in meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]ᵀ, with units in meters and
* radians.
- * @param nominalDt The time in seconds between each robot
- * loop.
*/
SwerveDrivePoseEstimator(
+ SwerveDriveKinematics& kinematics,
const Rotation2d& gyroAngle,
const wpi::array& modulePositions,
- const Pose2d& initialPose, SwerveDriveKinematics& kinematics,
- const wpi::array& stateStdDevs,
- const wpi::array& localMeasurementStdDevs,
- const wpi::array& visionMeasurementStdDevs,
- units::second_t nominalDt = 20_ms)
- : m_observer([](const Vectord& x,
- const Vectord& u) { return u; },
- [](const Vectord& x, const Vectord& u) {
- return x.template block(2, 0);
- },
- stateStdDevs, localMeasurementStdDevs,
- frc::AngleMean(2),
- frc::AngleMean(0),
- frc::AngleResidual(2),
- frc::AngleResidual(0), frc::AngleAdd(2),
- nominalDt),
- m_kinematics(kinematics),
- m_nominalDt(nominalDt) {
- SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
- // Create correction mechanism for vision measurements.
- m_visionCorrect = [&](const Vectord& u, const Vectord<3>& y) {
- m_observer.template Correct<3>(
- u, y,
- [](const Vectord& x, const Vectord& u) {
- return x.template block<3, 1>(0, 0);
- },
- m_visionContR, frc::AngleMean<3, States>(2), frc::AngleResidual<3>(2),
- frc::AngleResidual(2), frc::AngleAdd(2));
- };
-
- // Set initial state.
- Vectord xhat;
- auto poseVec = PoseTo3dVector(initialPose);
- xhat(0) = poseVec(0);
- xhat(1) = poseVec(1);
- xhat(2) = poseVec(2);
- for (size_t i = 0; i < NumModules; i++) {
- xhat(3 + i) = modulePositions[i].distance.value();
+ const Pose2d& initialPose, const wpi::array& stateStdDevs,
+ const wpi::array& visionMeasurementStdDevs)
+ : m_kinematics{kinematics},
+ m_odometry{kinematics, gyroAngle, modulePositions, initialPose} {
+ for (size_t i = 0; i < 3; ++i) {
+ m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
- m_observer.SetXhat(xhat);
- // Calculate offsets.
- m_gyroOffset = initialPose.Rotation() - gyroAngle;
- m_previousAngle = initialPose.Rotation();
+ SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Resets the robot's position on the field.
*
- * IF leftDistance and rightDistance are unspecified,
- * You NEED to reset your encoders (to zero).
- *
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
@@ -154,35 +96,16 @@ class SwerveDrivePoseEstimator {
const wpi::array& modulePositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
- m_observer.Reset();
+ m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
m_poseBuffer.Clear();
-
- Vectord xhat;
- auto poseVec = PoseTo3dVector(pose);
- xhat(0) = poseVec(0);
- xhat(1) = poseVec(1);
- xhat(2) = poseVec(2);
- for (size_t i = 0; i < NumModules; i++) {
- xhat(3 + i) = modulePositions[i].distance.value();
- }
- m_observer.SetXhat(xhat);
-
- m_prevTime = -1_s;
-
- m_gyroOffset = pose.Rotation() - gyroAngle;
- m_previousAngle = pose.Rotation();
}
/**
- * Gets the pose of the robot at the current time as estimated by the Extended
- * Kalman Filter.
+ * Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
- Pose2d GetEstimatedPosition() const {
- return Pose2d{m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
- Rotation2d{units::radian_t{m_observer.Xhat(2)}}};
- }
+ Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
/**
* Sets the pose estimator's trust of global measurements. This might be used
@@ -198,13 +121,26 @@ class SwerveDrivePoseEstimator {
*/
void SetVisionMeasurementStdDevs(
const wpi::array& visionMeasurementStdDevs) {
- // Create R (covariances) for vision measurements.
- m_visionContR = frc::MakeCovMatrix(visionMeasurementStdDevs);
+ wpi::array r{wpi::empty_array};
+ for (size_t i = 0; i < 3; ++i) {
+ r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (size_t row = 0; row < 3; ++row) {
+ if (m_q[row] == 0.0) {
+ m_visionK(row, row) = 0.0;
+ } else {
+ m_visionK(row, row) =
+ m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+ }
+ }
}
/**
- * Add a vision measurement to the Unscented Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the
+ * odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
@@ -226,16 +162,50 @@ class SwerveDrivePoseEstimator {
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
- if (auto sample = m_poseBuffer.Sample(timestamp)) {
- m_visionCorrect(Vectord::Zero(),
- PoseTo3dVector(GetEstimatedPosition().TransformBy(
- visionRobotPose - sample.value())));
+ // Step 1: Get the estimated pose from when the vision measurement was made.
+ auto sample = m_poseBuffer.Sample(timestamp);
+
+ if (!sample.has_value()) {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose
+ auto twist = sample.value().pose.Log(visionRobotPose);
+
+ // Step 3: 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.
+ frc::Vectord<3> k_times_twist =
+ m_visionK * frc::Vectord<3>{twist.dx.value(), twist.dy.value(),
+ twist.dtheta.value()};
+
+ // Step 4: 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().modulePostions,
+ sample.value().pose.Exp(scaledTwist));
+
+ // Step 6: 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.modulePostions);
}
}
/**
- * Adds a vision measurement to the Unscented Kalman Filter. This will correct
- * the odometry pose estimate while still accounting for measurement noise.
+ * Adds a vision measurement to the Kalman Filter. This will correct the
+ * odometry pose estimate while still accounting for measurement noise.
*
* This method can be called as infrequently as you want, as long as you are
* calling Update() every loop.
@@ -276,91 +246,137 @@ class SwerveDrivePoseEstimator {
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the Kalman Filter using only wheel encoder information. This should
+ * be called every loop.
*
* @param gyroAngle The current gyro angle.
- * @param moduleStates The current velocities and rotations of the swerve
- * modules.
* @param modulePositions The current distance and rotation measurements of
* the swerve modules.
- * @return The estimated pose of the robot in meters.
+ * @return The estimated robot pose in meters.
*/
Pose2d Update(
const Rotation2d& gyroAngle,
- const wpi::array& moduleStates,
const wpi::array& modulePositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
- moduleStates, modulePositions);
+ modulePositions);
}
/**
- * Updates the the Unscented Kalman Filter using only wheel encoder
- * information. This should be called every loop, and the correct loop period
- * must be passed into the constructor of this class.
+ * Updates the Kalman Filter using only wheel encoder information. This should
+ * be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
- * @param moduleStates The current velocities and rotations of the swerve
- * modules.
- * @param modulePositions The current distance travelled and rotations of
+ * @param modulePositions The current distance traveled and rotations of
* the swerve modules.
- * @return The estimated pose of the robot in meters.
+ * @return The estimated robot pose in meters.
*/
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
- const wpi::array& moduleStates,
const wpi::array& modulePositions) {
- auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
- m_prevTime = currentTime;
+ m_odometry.Update(gyroAngle, modulePositions);
- auto angle = gyroAngle + m_gyroOffset;
- auto omega = (angle - m_previousAngle).Radians() / dt;
+ wpi::array internalModulePositions{
+ wpi::empty_array};
- auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates);
- auto fieldRelativeSpeeds =
- Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy(
- angle);
-
- Vectord u;
- u(0) = fieldRelativeSpeeds.X().value();
- u(1) = fieldRelativeSpeeds.Y().value();
- u(2) = omega.value();
for (size_t i = 0; i < NumModules; i++) {
- u(3 + i) = moduleStates[i].speed.value();
+ internalModulePositions[i].distance = modulePositions[i].distance;
+ internalModulePositions[i].angle = modulePositions[i].angle;
}
- Vectord localY;
- localY(0) = angle.Radians().value();
- for (size_t i = 0; i < NumModules; i++) {
- localY(1 + i) = modulePositions[i].distance.value();
- }
-
- m_previousAngle = angle;
-
- m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
-
- m_observer.Predict(u, dt);
- m_observer.Correct(u, localY);
+ m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
+ internalModulePositions});
return GetEstimatedPosition();
}
private:
- UnscentedKalmanFilter m_observer;
+ 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 and rotations meaured at each module.
+ wpi::array modulePostions;
+
+ /**
+ * 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(
+ SwerveDriveKinematics& kinematics,
+ InterpolationRecord endValue, double i) const {
+ if (i < 0) {
+ return *this;
+ } else if (i > 1) {
+ return endValue;
+ } else {
+ // Find the new module distances.
+ wpi::array modulePositions{
+ wpi::empty_array};
+ // Find the distance between this measurement and the
+ // interpolated measurement.
+ wpi::array modulesDelta{
+ wpi::empty_array};
+
+ for (size_t i = 0; i < NumModules; i++) {
+ modulePositions[i].distance =
+ wpi::Lerp(this->modulePostions[i].distance,
+ endValue.modulePostions[i].distance, i);
+ modulePositions[i].angle =
+ wpi::Lerp(this->modulePostions[i].angle,
+ endValue.modulePostions[i].angle, i);
+
+ modulesDelta[i].distance =
+ modulePositions[i].distance - this->modulePostions[i].distance;
+ modulesDelta[i].angle = modulePositions[i].angle;
+ }
+
+ // Find the new gyro angle.
+ auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+ // Create a twist to represent this changed based on the interpolated
+ // sensor inputs.
+ auto twist = kinematics.ToTwist2d(modulesDelta);
+ twist.dtheta = (gyro - gyroAngle).Radians();
+
+ return {pose.Exp(twist), gyro, modulePositions};
+ }
+ }
+ };
+
SwerveDriveKinematics& m_kinematics;
- TimeInterpolatableBuffer m_poseBuffer{1.5_s};
- std::function& u, const Vectord<3>& y)>
- m_visionCorrect;
+ SwerveDriveOdometry m_odometry;
+ wpi::array m_q{wpi::empty_array};
+ Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
- Eigen::Matrix3d m_visionContR;
-
- units::second_t m_nominalDt;
- units::second_t m_prevTime = -1_s;
-
- Rotation2d m_gyroOffset;
- Rotation2d m_previousAngle;
+ TimeInterpolatableBuffer m_poseBuffer{
+ 1.5_s, [this](const InterpolationRecord& start,
+ const InterpolationRecord& end, double t) {
+ return start.Interpolate(this->m_kinematics, end, t);
+ }};
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index b946dee1f2..771fe846b0 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -120,6 +120,14 @@ class TimeInterpolatableBuffer {
return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
}
+ /**
+ * Grant access to the internal sample buffer. Used in Pose Estimation to
+ * replay odometry inputs stored within this buffer.
+ */
+ std::vector>& GetInternalBuffer() {
+ return m_pastSnapshots;
+ }
+
private:
units::second_t m_historySize;
std::vector> m_pastSnapshots;
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 4bf27ff6ac..930c7a6a02 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -6,6 +6,7 @@
#include
+#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/angle.h"
@@ -64,6 +65,20 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
+ /**
+ * Returns a twist from left and right distance deltas using
+ * forward kinematics.
+ *
+ * @param leftDistance The distance measured by the left encoder.
+ * @param rightDistance The distance measured by the right encoder.
+ * @return The resulting Twist2d.
+ */
+ constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
+ const units::meter_t rightDistance) const {
+ return {(leftDistance + rightDistance) / 2, 0_m,
+ (rightDistance - leftDistance) / trackWidth * 1_rad};
+ }
+
units::meter_t trackWidth;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index a1a89ca68c..cc198ac470 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -60,8 +60,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
- m_prevLeftDistance = 0_m;
- m_prevRightDistance = 0_m;
+ m_prevLeftDistance = leftDistance;
+ m_prevRightDistance = rightDistance;
}
/**
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
index 235d96f5a1..b69acebcd0 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
@@ -32,5 +32,22 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
* Distance driven by the rear-right wheel.
*/
units::meter_t rearRight = 0_m;
+
+ /**
+ * Checks equality between this MecanumDriveWheelPositions and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const MecanumDriveWheelPositions& other) const = default;
+
+ /**
+ * Checks inequality between this MecanumDriveWheelPositions and another
+ * object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const MecanumDriveWheelPositions& other) const = default;
};
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 59af152e3c..015c2c0038 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -66,11 +66,9 @@ class SwerveDriveOdometry {
/**
* Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method takes in the current time as
- * a parameter to calculate period (difference between two timestamps). The
- * period is used to calculate the change in distance from a velocity. This
- * also takes in an angle parameter which is used instead of the
- * angular rate that is calculated from forward kinematics.
+ * integration of the pose over time. This also takes in an angle parameter
+ * which is used instead of the angular rate that is calculated from forward
+ * kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The current position of all swerve modules. Please
@@ -90,7 +88,8 @@ class SwerveDriveOdometry {
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
- wpi::array m_previousModulePositions;
+ wpi::array m_previousModulePositions{
+ wpi::empty_array};
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 706c3f1806..64b46c1ff7 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -13,11 +13,15 @@ SwerveDriveOdometry::SwerveDriveOdometry(
SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle,
const wpi::array& modulePositions,
const Pose2d& initialPose)
- : m_kinematics(kinematics),
- m_pose(initialPose),
- m_previousModulePositions(modulePositions) {
+ : m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+ for (size_t i = 0; i < NumModules; i++) {
+ m_previousModulePositions[i] = {modulePositions[i].distance,
+ modulePositions[i].angle};
+ }
+
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
@@ -30,7 +34,10 @@ void SwerveDriveOdometry::ResetPosition(
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
- m_previousModulePositions = modulePositions;
+
+ for (size_t i = 0; i < NumModules; i++) {
+ m_previousModulePositions[i].distance = modulePositions[i].distance;
+ }
}
template
@@ -39,11 +46,13 @@ const Pose2d& frc::SwerveDriveOdometry::Update(
const wpi::array& modulePositions) {
auto moduleDeltas =
wpi::array(wpi::empty_array);
- for (size_t index = 0; index < modulePositions.size(); index++) {
+ for (size_t index = 0; index < NumModules; index++) {
auto lastPosition = m_previousModulePositions[index];
auto currentPosition = modulePositions[index];
moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
currentPosition.angle};
+
+ m_previousModulePositions[index].distance = modulePositions[index].distance;
}
auto angle = gyroAngle + m_gyroOffset;
@@ -55,7 +64,6 @@ const Pose2d& frc::SwerveDriveOdometry::Update(
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
- m_previousModulePositions = modulePositions;
return m_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/DifferentialDrivePoseEstimatorTest.java
index 97f4129d51..c3906d4de0 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -6,35 +6,37 @@ package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Nat;
+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.DifferentialDriveKinematics;
-import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
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 DifferentialDrivePoseEstimatorTest {
@Test
void testAccuracy() {
+ var kinematics = new DifferentialDriveKinematics(1);
+
var estimator =
new DifferentialDrivePoseEstimator(
+ kinematics,
new Rotation2d(),
0,
0,
new Pose2d(),
- new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
- new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
- new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
-
- var traj =
+ VecBuilder.fill(0.02, 0.02, 0.01),
+ VecBuilder.fill(0.1, 0.1, 0.1));
+ var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -42,67 +44,165 @@ class DifferentialDrivePoseEstimatorTest {
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(10, 5));
+ 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 DifferentialDriveKinematics(1);
- var rand = new Random(4915);
- final double dt = 0.02;
+ 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(
+ 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 DifferentialDriveKinematics kinematics,
+ final DifferentialDrivePoseEstimator 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;
+
+ estimator.resetPosition(
+ new Rotation2d(), leftDistanceMeters, rightDistanceMeters, startingPose);
+
+ var rand = new Random(3538);
+
double t = 0.0;
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+ System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
- double distanceLeft = 0.0;
- double distanceRight = 0.0;
+ final TreeMap visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
- Trajectory.State groundtruthState;
- DifferentialDriveWheelSpeeds input;
- while (t <= traj.getTotalTimeSeconds()) {
- groundtruthState = traj.sample(t);
- input =
- kinematics.toWheelSpeeds(
- new ChassisSpeeds(
- groundtruthState.velocityMetersPerSecond,
- 0.0,
- // ds/dt * dtheta/ds = dtheta/dt
- groundtruthState.velocityMetersPerSecond
- * groundtruthState.curvatureRadPerMeter));
+ while (t <= trajectory.getTotalTimeSeconds()) {
+ var groundTruthState = trajectory.sample(t);
- if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- var groundPose = groundtruthState.poseMeters;
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation()));
- lastVisionUpdateTime = 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);
}
- input.leftMetersPerSecond += rand.nextGaussian() * 0.01;
- input.rightMetersPerSecond += rand.nextGaussian() * 0.01;
+ // 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());
+ }
- distanceLeft += input.leftMetersPerSecond * dt;
- distanceRight += input.rightMetersPerSecond * dt;
+ var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+ var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+ leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt;
+ rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt;
- var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
var xHat =
estimator.updateWithTime(
t,
- groundtruthState.poseMeters.getRotation().plus(rotNoise),
- input,
- distanceLeft,
- distanceRight);
+ groundTruthState
+ .poseMeters
+ .getRotation()
+ .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+ .minus(trajectory.getInitialPose().getRotation()),
+ leftDistanceMeters,
+ rightDistanceMeters);
+
+ System.out.printf(
+ "%f, %f, %f, %f, %f, %f, %f\n",
+ t,
+ xHat.getX(),
+ xHat.getY(),
+ xHat.getRotation().getRadians(),
+ groundTruthState.poseMeters.getX(),
+ groundTruthState.poseMeters.getY(),
+ groundTruthState.poseMeters.getRotation().getRadians());
double error =
- groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+ groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -111,7 +211,20 @@ class DifferentialDrivePoseEstimatorTest {
t += dt;
}
- assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+ 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");
+ }
}
}
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
index e7eb17eb9e..02d2d52c42 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -9,14 +9,18 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
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 {
@@ -31,68 +35,155 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
+ kinematics,
new Rotation2d(),
wheelPositions,
new Pose2d(),
- kinematics,
- VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
- VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
- VecBuilder.fill(0.1, 0.1, 0.1));
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.45, 0.45, 0.1));
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
- new Pose2d(),
- new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
- new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
- new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
- new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
- new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
- new TrajectoryConfig(0.5, 2));
+ 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));
- var rand = new Random(5190);
+ 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);
- final double dt = 0.02;
double t = 0.0;
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+ System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+
+ final TreeMap visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
- if (lastVisionUpdateTime + visionUpdateRate < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
+ // 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)));
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundTruthState.poseMeters.getTranslation().getY()
- + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.1)
- .plus(groundTruthState.poseMeters.getRotation()));
-
- lastVisionUpdateTime = t;
+ visionUpdateQueue.put(t, newVisionPose);
}
- var wheelSpeeds =
- kinematics.toWheelSpeeds(
- new ChassisSpeeds(
- groundTruthState.velocityMetersPerSecond,
- 0,
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.curvatureRadPerMeter));
+ // 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());
+ }
- wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+ var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+ var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
@@ -105,108 +196,19 @@ class MecanumDrivePoseEstimatorTest {
groundTruthState
.poseMeters
.getRotation()
- .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
- wheelSpeeds,
+ .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(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.125, "Incorrect max error");
- }
-
- @Test
- void testAccuracyFacingXAxis() {
- 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(
- new Rotation2d(),
- wheelPositions,
- new Pose2d(),
- kinematics,
- VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
- VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
- VecBuilder.fill(0.1, 0.1, 0.1));
-
- var trajectory =
- TrajectoryGenerator.generateTrajectory(
- List.of(
- new Pose2d(),
- new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
- new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
- new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
- new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
- new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
- new TrajectoryConfig(0.5, 2));
-
- var rand = new Random(5190);
-
- final double dt = 0.02;
- double t = 0.0;
-
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
-
- double maxError = Double.NEGATIVE_INFINITY;
- double errorSum = 0;
- while (t <= trajectory.getTotalTimeSeconds()) {
- var groundTruthState = trajectory.sample(t);
-
- if (lastVisionUpdateTime + visionUpdateRate < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
-
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundTruthState.poseMeters.getTranslation().getY()
- + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.1)
- .plus(groundTruthState.poseMeters.getRotation()));
-
- lastVisionUpdateTime = t;
- }
-
- var wheelSpeeds =
- kinematics.toWheelSpeeds(
- new ChassisSpeeds(
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.poseMeters.getRotation().getCos(),
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.poseMeters.getRotation().getSin(),
- 0));
-
- wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
- wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
-
- 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, new Rotation2d(rand.nextGaussian() * 0.05), wheelSpeeds, wheelPositions);
+ System.out.printf(
+ "%f, %f, %f, %f, %f, %f, %f\n",
+ t,
+ xHat.getX(),
+ xHat.getY(),
+ xHat.getRotation().getRadians(),
+ groundTruthState.poseMeters.getX(),
+ groundTruthState.poseMeters.getY(),
+ groundTruthState.poseMeters.getRotation().getRadians());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
@@ -219,7 +221,19 @@ class MecanumDrivePoseEstimatorTest {
}
assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+ 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");
+ }
}
}
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
index 27b07c61d0..fde2c39e9f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -6,20 +6,21 @@ package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
-import edu.wpi.first.math.Nat;
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.numbers.N5;
-import edu.wpi.first.math.numbers.N7;
+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 {
@@ -38,17 +39,13 @@ class SwerveDrivePoseEstimatorTest {
var br = new SwerveModulePosition();
var estimator =
- new SwerveDrivePoseEstimator(
- Nat.N7(),
- Nat.N7(),
- Nat.N5(),
+ new SwerveDrivePoseEstimator(
+ kinematics,
new Rotation2d(),
new SwerveModulePosition[] {fl, fr, bl, br},
new Pose2d(),
- kinematics,
- VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
- VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),
- VecBuilder.fill(0.1, 0.1, 0.1));
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.5, 0.5, 0.5));
var trajectory =
TrajectoryGenerator.generateTrajectory(
@@ -58,88 +55,28 @@ class SwerveDrivePoseEstimatorTest {
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(0.5, 2));
+ new TrajectoryConfig(2, 2));
- var rand = new Random(4915);
-
- final double dt = 0.02;
- double t = 0.0;
-
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
-
- double maxError = Double.NEGATIVE_INFINITY;
- double errorSum = 0;
- while (t <= trajectory.getTotalTimeSeconds()) {
- var groundTruthState = trajectory.sample(t);
-
- if (lastVisionUpdateTime + visionUpdateRate < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
-
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundTruthState.poseMeters.getTranslation().getY()
- + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.1)
- .plus(groundTruthState.poseMeters.getRotation()));
-
- lastVisionUpdateTime = t;
- }
-
- var moduleStates =
- kinematics.toSwerveModuleStates(
- new ChassisSpeeds(
- groundTruthState.velocityMetersPerSecond,
- 0.0,
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.curvatureRadPerMeter));
- for (var moduleState : moduleStates) {
- moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
- moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
- }
-
- fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt;
- fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt;
- bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt;
- br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt;
-
- fl.angle = moduleStates[0].angle;
- fr.angle = moduleStates[1].angle;
- bl.angle = moduleStates[2].angle;
- br.angle = moduleStates[3].angle;
-
- var xHat =
- estimator.updateWithTime(
- t,
- groundTruthState
- .poseMeters
- .getRotation()
- .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
- moduleStates,
- new SwerveModulePosition[] {fl, fr, bl, br});
-
- double error =
- groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
- }
-
- assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+ 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 testAccuracyFacingXAxis() {
+ void testBadInitialPose() {
var kinematics =
new SwerveDriveKinematics(
new Translation2d(1, 1),
@@ -153,18 +90,13 @@ class SwerveDrivePoseEstimatorTest {
var br = new SwerveModulePosition();
var estimator =
- new SwerveDrivePoseEstimator(
- Nat.N7(),
- Nat.N7(),
- Nat.N5(),
+ new SwerveDrivePoseEstimator(
+ kinematics,
new Rotation2d(),
new SwerveModulePosition[] {fl, fr, bl, br},
- new Pose2d(),
- kinematics,
- VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
- VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),
- VecBuilder.fill(0.1, 0.1, 0.1));
-
+ 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(
@@ -173,76 +105,137 @@ class SwerveDrivePoseEstimatorTest {
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
- new TrajectoryConfig(0.5, 2));
+ new TrajectoryConfig(2, 2));
- var rand = new Random(4915);
+ 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);
- final double dt = 0.02;
double t = 0.0;
- final double visionUpdateRate = 0.1;
- Pose2d lastVisionPose = null;
- double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+ System.out.print(
+ "time, est_x, est_y, est_theta, true_x, true_y, true_theta, "
+ + "distance_1, distance_2, distance_3, distance_4, "
+ + "angle_1, angle_2, angle_3, angle_4\n");
+
+ final TreeMap visionUpdateQueue = new TreeMap<>();
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
- if (lastVisionUpdateTime + visionUpdateRate < t) {
- if (lastVisionPose != null) {
- estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
+ // 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)));
- lastVisionPose =
- new Pose2d(
- new Translation2d(
- groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
- groundTruthState.poseMeters.getTranslation().getY()
- + rand.nextGaussian() * 0.1),
- new Rotation2d(rand.nextGaussian() * 0.1)
- .plus(groundTruthState.poseMeters.getRotation()));
-
- lastVisionUpdateTime = t;
+ visionUpdateQueue.put(t, newVisionPose);
}
- var moduleStates =
- kinematics.toSwerveModuleStates(
- new ChassisSpeeds(
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.poseMeters.getRotation().getCos(),
- groundTruthState.velocityMetersPerSecond
- * groundTruthState.poseMeters.getRotation().getSin(),
- 0.0));
- for (var moduleState : moduleStates) {
- moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
- moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+ // 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());
}
- fl.distanceMeters +=
- groundTruthState.velocityMetersPerSecond * dt
- + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
- fr.distanceMeters +=
- groundTruthState.velocityMetersPerSecond * dt
- + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
- bl.distanceMeters +=
- groundTruthState.velocityMetersPerSecond * dt
- + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
- br.distanceMeters +=
- groundTruthState.velocityMetersPerSecond * dt
- + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+ var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
- fl.angle = groundTruthState.poseMeters.getRotation();
- fr.angle = groundTruthState.poseMeters.getRotation();
- bl.angle = groundTruthState.poseMeters.getRotation();
- br.angle = groundTruthState.poseMeters.getRotation();
+ 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,
- new Rotation2d(rand.nextGaussian() * 0.05),
- moduleStates,
- new SwerveModulePosition[] {fl, fr, bl, br});
+ groundTruthState
+ .poseMeters
+ .getRotation()
+ .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+ .minus(trajectory.getInitialPose().getRotation()),
+ positions);
+
+ System.out.printf(
+ "%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
+ t,
+ xHat.getX(),
+ xHat.getY(),
+ xHat.getRotation().getRadians(),
+ groundTruthState.poseMeters.getX(),
+ groundTruthState.poseMeters.getY(),
+ groundTruthState.poseMeters.getRotation().getRadians(),
+ positions[0].distanceMeters,
+ positions[1].distanceMeters,
+ positions[2].distanceMeters,
+ positions[3].distanceMeters,
+ positions[0].angle.getRadians(),
+ positions[1].angle.getRadians(),
+ positions[2].angle.getRadians(),
+ positions[3].angle.getRadians());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
@@ -255,7 +248,19 @@ class SwerveDrivePoseEstimatorTest {
}
assertEquals(
- 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
- assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+ 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");
+ }
}
}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
index 5b07687f08..a2d8a350c0 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
@@ -178,6 +178,14 @@ class SwerveDriveOdometryTest {
t += dt;
}
+ assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
+ assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
+ assertEquals(
+ Math.PI / 4,
+ odometry.getPoseMeters().getRotation().getRadians(),
+ 10 * Math.PI / 180,
+ "Incorrect Final Theta");
+
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
@@ -253,6 +261,14 @@ class SwerveDriveOdometryTest {
t += dt;
}
+ assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
+ assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
+ assertEquals(
+ 0.0,
+ odometry.getPoseMeters().getRotation().getRadians(),
+ 10 * Math.PI / 180,
+ "Incorrect Final Theta");
+
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 550f376daf..1d623ca591 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -4,6 +4,8 @@
#include
#include
+#include
+#include
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
@@ -16,68 +18,89 @@
#include "units/length.h"
#include "units/time.h"
-TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
- frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d{},
- 0_m,
- 0_m,
- frc::Pose2d{},
- {0.02, 0.02, 0.01, 0.02, 0.02},
- {0.01, 0.01, 0.001},
- {0.1, 0.1, 0.01}};
+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;
- 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(10_mps, 5.0_mps_sq));
-
- frc::DifferentialDriveKinematics kinematics{1.0_m};
+ estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance,
+ startingPose);
std::default_random_engine generator;
std::normal_distribution distribution(0.0, 1.0);
- units::second_t dt = 0.02_s;
- units::second_t t = 0.0_s;
+ units::second_t t = 0_s;
- units::meter_t leftDistance = 0_m;
- units::meter_t rightDistance = 0_m;
-
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits::max()};
+ std::vector> visionPoses;
+ std::vector>
+ visionLog;
double maxError = -std::numeric_limits::max();
double errorSum = 0;
- while (t <= trajectory.TotalTime()) {
- auto groundTruthState = trajectory.Sample(t);
- auto input = kinematics.ToWheelSpeeds(
- {groundTruthState.velocity, 0_mps,
- groundTruthState.velocity * groundTruthState.curvature});
+ if (debug) {
+ fmt::print(
+ "time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, "
+ "right\n");
+ }
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d{}) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d{
- frc::Translation2d{distribution(generator) * 0.1 * 1_m,
- distribution(generator) * 0.1 * 1_m},
- frc::Rotation2d{distribution(generator) * 0.01 * 1_rad}};
+ while (t < trajectory.TotalTime()) {
+ frc::Trajectory::State groundTruthState = trajectory.Sample(t);
- lastVisionUpdateTime = 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});
}
- leftDistance += input.left * distribution(generator) * 0.01 * dt;
- rightDistance += input.right * distribution(generator) * 0.01 * dt;
+ // 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);
+
+ leftDistance += wheelSpeeds.left * dt;
+ rightDistance += wheelSpeeds.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
- frc::Rotation2d{units::radian_t{distribution(generator) * 0.001}},
- input, leftDistance, rightDistance);
+ frc::Rotation2d{distribution(generator) * 0.05_rad} -
+ trajectory.InitialPose().Rotation(),
+ leftDistance, rightDistance);
+
+ 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());
+ }
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
@@ -91,7 +114,96 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
t += dt;
}
- EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
- 0.05);
- EXPECT_NEAR(0.0, maxError, 0.125);
+ 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) {
+ EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05);
+ EXPECT_LT(maxError, 0.2);
+ }
+}
+
+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}};
+
+ 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_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(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}};
+
+ 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_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);
+ }
+ }
}
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index e0bbc94dcc..13dc5aa091 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -4,6 +4,7 @@
#include
#include
+#include
#include "frc/estimator/MecanumDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
@@ -11,63 +12,66 @@
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
-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}};
+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{};
- frc::MecanumDriveWheelPositions wheelPositions;
-
- frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
- wheelPositions,
- frc::Pose2d{},
- kinematics,
- {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
- {0.05, 0.05, 0.05, 0.05, 0.05},
- {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},
- 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(5.0_mps, 2.0_mps_sq));
+ estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose);
std::default_random_engine generator;
std::normal_distribution distribution(0.0, 1.0);
- units::second_t dt = 0.02_s;
units::second_t t = 0_s;
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits::max()};
-
- std::vector visionPoses;
+ 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);
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d{}) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d{
- frc::Translation2d{distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m},
- frc::Rotation2d{distribution(generator) * 0.1 * 1_rad}};
- visionPoses.push_back(lastVisionPose);
- lastVisionUpdateTime = 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});
}
- auto wheelSpeeds = kinematics.ToWheelSpeeds(
- {groundTruthState.velocity, 0_mps,
- groundTruthState.velocity * groundTruthState.curvature});
+ // 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;
@@ -77,8 +81,18 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
- frc::Rotation2d{distribution(generator) * 0.05_rad},
- wheelSpeeds, wheelPositions);
+ 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();
@@ -91,89 +105,104 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
t += dt;
}
- EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
- EXPECT_LT(maxError, 0.125);
+ 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) {
+ EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
+ EXPECT_LT(maxError, 0.2);
+ }
}
-TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingXAxis) {
+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{frc::Rotation2d{},
- wheelPositions,
- frc::Pose2d{},
- kinematics,
- {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
- {0.05, 0.05, 0.05, 0.05, 0.05},
- {0.1, 0.1, 0.1}};
+ 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(5.0_mps, 2.0_mps_sq));
+ frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
- std::default_random_engine generator;
- std::normal_distribution distribution(0.0, 1.0);
-
- units::second_t dt = 0.02_s;
- units::second_t t = 0_s;
-
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits::max()};
-
- std::vector visionPoses;
-
- double maxError = -std::numeric_limits::max();
- double errorSum = 0;
-
- while (t < trajectory.TotalTime()) {
- frc::Trajectory::State groundTruthState = trajectory.Sample(t);
-
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d{}) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d{
- frc::Translation2d{distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m},
- frc::Rotation2d{distribution(generator) * 0.1 * 1_rad}};
- visionPoses.push_back(lastVisionPose);
- lastVisionUpdateTime = t;
- }
-
- auto wheelSpeeds = kinematics.ToWheelSpeeds(
- {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
- groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
- 0_rad_per_s});
-
- 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, frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelSpeeds,
- wheelPositions);
- double error = groundTruthState.pose.Translation()
- .Distance(xhat.Translation())
- .value();
-
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
- }
-
- EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
- EXPECT_LT(maxError, 0.125);
+ 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);
+ }
+ }
}
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index e76afd75fe..554ca5943e 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -3,7 +3,11 @@
// the WPILib BSD license file in the root directory of this project.
#include
+#include
#include
+#include
+
+#include
#include "frc/estimator/SwerveDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
@@ -11,6 +15,128 @@
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.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) {
+ 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},
@@ -22,88 +148,28 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
- frc::Rotation2d{},
- {fl, fr, bl, br},
- frc::Pose2d{},
- kinematics,
- {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
- {0.05, 0.05, 0.05, 0.05, 0.05},
- {0.1, 0.1, 0.1}};
+ 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(5.0_mps, 2.0_mps_sq));
+ frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
- std::default_random_engine generator;
- std::normal_distribution distribution(0.0, 1.0);
-
- units::second_t dt = 0.02_s;
- units::second_t t = 0_s;
-
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits::max()};
-
- std::vector visionPoses;
-
- double maxError = -std::numeric_limits::max();
- double errorSum = 0;
-
- while (t < trajectory.TotalTime()) {
- frc::Trajectory::State groundTruthState = trajectory.Sample(t);
-
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d{}) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m},
- frc::Rotation2d{distribution(generator) * 0.1_rad}};
- visionPoses.push_back(lastVisionPose);
- lastVisionUpdateTime = t;
- }
-
- auto moduleStates = kinematics.ToSwerveModuleStates(
- {groundTruthState.velocity, 0_mps,
- groundTruthState.velocity * groundTruthState.curvature});
-
- fl.distance += moduleStates[0].speed * dt;
- fr.distance += moduleStates[1].speed * dt;
- bl.distance += moduleStates[2].speed * dt;
- br.distance += moduleStates[3].speed * dt;
-
- fl.angle = moduleStates[0].angle;
- fr.angle = moduleStates[1].angle;
- bl.angle = moduleStates[2].angle;
- br.angle = moduleStates[3].angle;
-
- auto xhat = estimator.UpdateWithTime(
- t,
- groundTruthState.pose.Rotation() +
- frc::Rotation2d{distribution(generator) * 0.05_rad},
- moduleStates, {fl, fr, bl, br});
- double error = groundTruthState.pose.Translation()
- .Distance(xhat.Translation())
- .value();
-
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
- }
-
- EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
- EXPECT_LT(maxError, 0.125);
+ 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, AccuracyFacingXAxis) {
+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}};
@@ -114,86 +180,38 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
- frc::Rotation2d{},
- {fl, fr, bl, br},
- frc::Pose2d{},
- kinematics,
- {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
- {0.05, 0.05, 0.05, 0.05, 0.05},
- {0.1, 0.1, 0.1}};
+ 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(5.0_mps, 2.0_mps_sq));
+ frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
- std::default_random_engine generator;
- std::normal_distribution distribution(0.0, 1.0);
+ 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};
- units::second_t dt = 0.02_s;
- units::second_t t = 0_s;
+ auto initial_pose =
+ trajectory.InitialPose() +
+ frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
+ pose_offset.Sin() * 1_m},
+ heading_offset};
- units::second_t kVisionUpdateRate = 0.1_s;
- frc::Pose2d lastVisionPose;
- units::second_t lastVisionUpdateTime{-std::numeric_limits::max()};
-
- std::vector visionPoses;
-
- double maxError = -std::numeric_limits::max();
- double errorSum = 0;
-
- while (t < trajectory.TotalTime()) {
- frc::Trajectory::State groundTruthState = trajectory.Sample(t);
-
- if (lastVisionUpdateTime + kVisionUpdateRate < t) {
- if (lastVisionPose != frc::Pose2d{}) {
- estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
- }
- lastVisionPose =
- groundTruthState.pose +
- frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
- distribution(generator) * 0.1_m},
- frc::Rotation2d{distribution(generator) * 0.1_rad}};
- visionPoses.push_back(lastVisionPose);
- lastVisionUpdateTime = t;
+ 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);
}
-
- auto moduleStates = kinematics.ToSwerveModuleStates(
- {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
- groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
- 0_rad_per_s});
-
- fl.distance += groundTruthState.velocity * dt +
- 0.5 * groundTruthState.acceleration * dt * dt;
- fr.distance += groundTruthState.velocity * dt +
- 0.5 * groundTruthState.acceleration * dt * dt;
- bl.distance += groundTruthState.velocity * dt +
- 0.5 * groundTruthState.acceleration * dt * dt;
- br.distance += groundTruthState.velocity * dt +
- 0.5 * groundTruthState.acceleration * dt * dt;
-
- fl.angle = groundTruthState.pose.Rotation();
- fr.angle = groundTruthState.pose.Rotation();
- bl.angle = groundTruthState.pose.Rotation();
- br.angle = groundTruthState.pose.Rotation();
-
- auto xhat = estimator.UpdateWithTime(
- t, frc::Rotation2d{distribution(generator) * 0.05_rad}, moduleStates,
- {fl, fr, bl, br});
- double error = groundTruthState.pose.Translation()
- .Distance(xhat.Translation())
- .value();
-
- if (error > maxError) {
- maxError = error;
- }
- errorSum += error;
-
- t += dt;
}
-
- EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
- EXPECT_LT(maxError, 0.125);
}