Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use Odometry for internal state in Pose Estimation (#4668)

Browse Source 
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.

Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2022-12-02 11:36:10 -05:00
committed by GitHub
parent 68dba92630
commit e22d8cc343
35 changed files with 2288 additions and 1884 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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.
*
* <p>{@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.
* <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop.
*
* <p>The state-space system used internally has the following states (x), inputs (u), and outputs
* (y):
* <p>{@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.
*
* <p><strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate system
* containing x position, y position, heading, left encoder distance, and right encoder distance.
* <p>The state-space system used internally has the following states (x), and outputs (y):
*
* <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
* in the field coordinate system.
*
* <p>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.
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
* heading; or <strong>y = [dist_l, dist_r, theta] </strong> containing left encoder position, right
* encoder position, and gyro heading.
* heading.
*/
public class DifferentialDrivePoseEstimator {
final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing
private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
private final DifferentialDriveKinematics m_kinematics;
private final DifferentialDriveOdometry m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private final double m_nominalDt; // Seconds
private double m_prevTimeSeconds = -1.0;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionContR;
private final TimeInterpolatableBuffer<InterpolationRecord> 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<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> 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<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs,
Matrix<N3, N1> 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<N3, N1> 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<N5, N1> f(Matrix<N5, N1> x, Matrix<N3, N1> 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.
*
* <p>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<Double, InterpolationRecord> 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.
*
* <p>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<N5, N1> 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<InterpolationRecord> {
// 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);
}
}
}

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

@@ -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}.
*
* <p>{@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)}.
* <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop.
*
* <p>{@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.
*
* <p>The state-space system used internally has the following states (x), inputs (u), and outputs
* (y):
* <p>The state-space system used internally has the following states (x) and outputs (y):
*
* <p><strong> x = [x, y, theta, s_fl, s_fr, s_rl, s_rr]ᵀ </strong> 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.
*
* <p><strong> u = [v_x, v_y, omega, v_fl, v_fr, v_rl, v_rr]ᵀ </strong> 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.
* <p><strong> x = [x, y, theta]ᵀ </strong> 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.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
* heading; or <strong> y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ </strong> 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<N7, N7, N5> m_observer;
private final MecanumDriveKinematics m_kinematics;
private final BiConsumer<Matrix<N7, N1>, Matrix<N3, N1>> m_visionCorrect;
private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
private final MecanumDriveOdometry m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private final double m_nominalDt; // Seconds
private double m_prevTimeSeconds = -1.0;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionContR;
private final TimeInterpolatableBuffer<InterpolationRecord> 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<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> 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<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
Matrix<N3, N1> 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<N3, N1> 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.
*
* <p>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<Double, InterpolationRecord> 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.
*
* <p>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<InterpolationRecord> {
// 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);
}
}
}

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

@@ -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}.
*
* <p>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.
*
* <p>{@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)}.
* <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
*
* <p>{@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.
*
* <p>The state-space system used internally has the following states (x), inputs (u), and outputs
* (y):
* <p>The state-space system used internally has the following states (x) and outputs (y):
*
* <p><strong> x = [x, y, theta, s_0, ..., s_n]ᵀ </strong> in the field coordinate system containing
* x position, y position, and heading, followed by the distance travelled by each wheel.
*
* <p><strong> u = [v_x, v_y, omega, v_0, ... v_n]ᵀ </strong> containing x velocity, y velocity, and
* angular rate in the field coordinate system, followed by the velocity measured at each wheel.
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
* heading; or <strong> y = [theta, s_0, ..., s_n]ᵀ </strong> containing gyro heading, followed by
* the distance travelled by each wheel.
* heading.
*/
public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Outputs extends Num> {
private final UnscentedKalmanFilter<States, Inputs, Outputs> m_observer;
public class SwerveDrivePoseEstimator {
private final SwerveDriveKinematics m_kinematics;
private final BiConsumer<Matrix<Inputs, N1>, Matrix<N3, N1>> m_visionCorrect;
private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
private final SwerveDriveOdometry m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private final int m_numModules;
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private final Nat<States> m_states;
private final Nat<Inputs> m_inputs;
private final Nat<Outputs> m_outputs;
private final double m_nominalDt; // Seconds
private double m_prevTimeSeconds = -1.0;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionContR;
private final TimeInterpolatableBuffer<InterpolationRecord> 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> states,
Nat<Inputs> inputs,
Nat<Outputs> outputs,
SwerveDriveKinematics kinematics,
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> localMeasurementStdDevs,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> 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> states,
Nat<Inputs> inputs,
Nat<Outputs> outputs,
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> localMeasurementStdDevs,
Matrix<N3, N1> 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<States, N1> xhat = new Matrix<States, N1>(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<States extends Num, Inputs extends Num, Ou
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> 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<States extends Num, Inputs extends Num, Ou
public void resetPosition(
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
m_observer.reset();
m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
m_poseBuffer.clear();
var poseVec = StateSpaceUtil.poseTo3dVector(poseMeters);
Matrix<States, N1> xhat = new Matrix<States, N1>(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.
*
* <p>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<States extends Num, Inputs extends Num, Ou
* 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 Matrix<Inputs, N1>(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<Double, InterpolationRecord> 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.
*
* <p>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<States extends Num, Inputs extends Num, Ou
}
/**
* 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 measurements and rotations of the swerve modules.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(
Rotation2d gyroAngle,
SwerveModuleState[] moduleStates,
SwerveModulePosition[] modulePositions) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates, modulePositions);
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, 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 currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param moduleStates The current velocities and rotations of the swerve modules.
* @param modulePositions The current distance measurements and rotations of the swerve modules.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(
double currentTimeSeconds,
Rotation2d gyroAngle,
SwerveModuleState[] moduleStates,
SwerveModulePosition[] modulePositions) {
double dt = m_prevTimeSeconds >= 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<Inputs, N1>(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<Outputs, N1>(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<InterpolationRecord> {
// 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);
}
}
}

10
wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
View File

@@ -134,6 +134,16 @@ public final class TimeInterpolatableBuffer<T> {
}
}
/**
* 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<Double, T> getInternalBuffer() {
return m_pastSnapshots;
}
public interface InterpolateFunction<T> {
/**
* Return the interpolated value. This object is assumed to be the starting position, or lower

17
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
View File

@@ -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);
}
}

7
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
View File

@@ -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;
}
}