Package edu.wpi.first.math.estimator

Class DifferentialDrivePoseEstimator

java.lang.Object

edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator

public class DifferentialDrivePoseEstimator
extends Object

This class wraps an 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 DifferentialDriveOdometry; in fact, if you never call addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d, double) and only call update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds, double, double) then this will behave exactly the same as DifferentialDriveOdometry.

update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds, double, double) should be called every robot loop (if your robot loops are faster than the default then you should change the nominal delta time.) addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d, double) can be called as infrequently as you want; if you never call it then this class will behave exactly like regular encoder odometry.

The state-space system used internally has the following states (x), inputs (u), 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_l, v_r, dtheta]ᵀ containing left wheel velocity, right wheel velocity, and change in gyro heading.

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.

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.

Constructor Summary

Constructors

Constructor	Description
DifferentialDrivePoseEstimator(Rotation2d gyroAngle, Pose2d initialPoseMeters, Matrix<N5,N1> stateStdDevs, Matrix<N3,N1> localMeasurementStdDevs, Matrix<N3,N1> visionMeasurementStdDevs)	Constructs a DifferentialDrivePoseEstimator.
DifferentialDrivePoseEstimator(Rotation2d gyroAngle, Pose2d initialPoseMeters, Matrix<N5,N1> stateStdDevs, Matrix<N3,N1> localMeasurementStdDevs, Matrix<N3,N1> visionMeasurementStdDevs, double nominalDtSeconds)	Constructs a DifferentialDrivePoseEstimator.

Method Summary

Modifier and Type	Method	Description
void	addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds)	Add a vision measurement to the Unscented Kalman Filter.
void	addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)	Add a vision measurement to the Unscented Kalman Filter.
Pose2d	getEstimatedPosition()	Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
void	resetPosition(Pose2d poseMeters, Rotation2d gyroAngle)	Resets the robot's position on the field.
void	setVisionMeasurementStdDevs(Matrix<N3,N1> visionMeasurementStdDevs)	Sets the pose estimator's trust of global measurements.
Pose2d	update(Rotation2d gyroAngle, DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond, double distanceLeftMeters, double distanceRightMeters)	Updates the the Unscented Kalman Filter using only wheel encoder information.
Pose2d	updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond, double distanceLeftMeters, double distanceRightMeters)	Updates the the Unscented Kalman Filter using only wheel encoder information.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Details

DifferentialDrivePoseEstimator

public DifferentialDrivePoseEstimator(Rotation2d gyroAngle, Pose2d initialPoseMeters, Matrix<N5,N1> stateStdDevs, Matrix<N3,N1> localMeasurementStdDevs, Matrix<N3,N1> visionMeasurementStdDevs)

Constructs a DifferentialDrivePoseEstimator.

Parameters:

gyroAngle - The current gyro angle.

initialPoseMeters - The starting pose estimate.

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.

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.

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.

DifferentialDrivePoseEstimator

public DifferentialDrivePoseEstimator(Rotation2d gyroAngle, Pose2d initialPoseMeters, Matrix<N5,N1> stateStdDevs, Matrix<N3,N1> localMeasurementStdDevs, Matrix<N3,N1> visionMeasurementStdDevs, double nominalDtSeconds)

Constructs a DifferentialDrivePoseEstimator.

Parameters:

gyroAngle - The current gyro angle.

initialPoseMeters - The starting pose estimate.

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.

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.

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.

nominalDtSeconds - The time in seconds between each robot loop.

Method Details

setVisionMeasurementStdDevs

public void setVisionMeasurementStdDevs(Matrix<N3,N1> visionMeasurementStdDevs)

Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases.

Parameters:

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.

resetPosition

public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle)

Resets the robot's position on the field.

You NEED to reset your encoders (to zero) when calling this method.

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.

Parameters:

poseMeters - The position on the field that your robot is at.

gyroAngle - The angle reported by the gyroscope.

getEstimatedPosition

public Pose2d getEstimatedPosition()

Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.

Returns:

The estimated robot pose in meters.

addVisionMeasurement

public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds)

Add a vision measurement to the Unscented 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(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds, double, double) every loop.

Parameters:

visionRobotPoseMeters - The pose of the robot as measured by the vision camera.

timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling updateWithTime(double, edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds, double, double) 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.

addVisionMeasurement

public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)

Add a vision measurement to the Unscented 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(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds, double, double) every loop.

Note that the vision measurement standard deviations passed into this method will continue to apply to future measurements until a subsequent call to setVisionMeasurementStdDevs(Matrix) or this method.

Parameters:

visionRobotPoseMeters - The pose of the robot as measured by the vision camera.

timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling updateWithTime(double, edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds, double, double) 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.

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.

update

public Pose2d update(Rotation2d gyroAngle, DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond, double distanceLeftMeters, double distanceRightMeters)

Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this should be called every loop.

Parameters:

gyroAngle - The current gyro angle.

wheelVelocitiesMetersPerSecond - The velocities of the wheels in meters per second.

distanceLeftMeters - The total distance travelled by the left wheel in meters since the last time you called resetPosition(edu.wpi.first.math.geometry.Pose2d, edu.wpi.first.math.geometry.Rotation2d).

distanceRightMeters - The total distance travelled by the right wheel in meters since the last time you called resetPosition(edu.wpi.first.math.geometry.Pose2d, edu.wpi.first.math.geometry.Rotation2d).

Returns:

The estimated pose of the robot in meters.

updateWithTime

public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond, double distanceLeftMeters, double distanceRightMeters)

Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this should be called every loop.

Parameters:

currentTimeSeconds - Time at which this method was called, in seconds.

gyroAngle - The current gyro angle.

wheelVelocitiesMetersPerSecond - The velocities of the wheels in meters per second.

distanceLeftMeters - The total distance travelled by the left wheel in meters since the last time you called resetPosition(edu.wpi.first.math.geometry.Pose2d, edu.wpi.first.math.geometry.Rotation2d).

distanceRightMeters - The total distance travelled by the right wheel in meters since the last time you called resetPosition(edu.wpi.first.math.geometry.Pose2d, edu.wpi.first.math.geometry.Rotation2d).

Returns:

The estimated pose of the robot in meters.