Class DifferentialDrivePoseEstimator

java.lang.Object

org.wpilib.math.estimator.PoseEstimator<DifferentialDriveWheelPositions>

org.wpilib.math.estimator.DifferentialDrivePoseEstimator

public class DifferentialDrivePoseEstimator
extends PoseEstimator<DifferentialDriveWheelPositions>

This class wraps Differential Drive Odometry to fuse latency-compensated vision measurements with differential drive encoder measurements. It is intended to be a drop-in replacement for DifferentialDriveOdometry; in fact, if you never call PoseEstimator.addVisionMeasurement(Pose2d, double) and only call update(Rotation2d, double, double) then this will behave exactly the same as DifferentialDriveOdometry.

update(Rotation2d, double, double) should be called every robot loop.

PoseEstimator.addVisionMeasurement(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.

Constructor Summary

Constructors

Constructor	Description
DifferentialDrivePoseEstimator(Rotation2d gyroAngle, double leftDistance, double rightDistance, Pose2d initialPose)	Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and vision measurements.
DifferentialDrivePoseEstimator(Rotation2d gyroAngle, double leftDistance, double rightDistance, Pose2d initialPose, Matrix<N3,N1> stateStdDevs, Matrix<N3,N1> visionMeasurementStdDevs)	Constructs a DifferentialDrivePoseEstimator.

Method Summary

Modifier and Type	Method	Description
void	resetPosition(Rotation2d gyroAngle, double leftPosition, double rightPosition, Pose2d pose)	Resets the robot's position on the field.
Pose2d	update(Rotation2d gyroAngle, double distanceLeft, double distanceRight)	Updates the pose estimator with wheel encoder and gyro information.
Pose2d	updateWithTime(double currentTime, Rotation2d gyroAngle, double distanceLeft, double distanceRight)	Updates the pose estimator with wheel encoder and gyro information.

Methods inherited from class PoseEstimator

addVisionMeasurement, addVisionMeasurement, getEstimatedPosition, resetPose, resetPosition, resetRotation, resetTranslation, sampleAt, setVisionMeasurementStdDevs, update, updateWithTime

Methods inherited from class Object

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

Constructor Details

DifferentialDrivePoseEstimator

public DifferentialDrivePoseEstimator(Rotation2d gyroAngle,
 double leftDistance,
 double rightDistance,
 Pose2d initialPose)

Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and vision measurements.

The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for y, and 0.01 radians for heading. The default standard deviations of the vision measurements are 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.

Parameters:

gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.

leftDistance - The distance traveled by the left encoder in meters.

rightDistance - The distance traveled by the right encoder in meters.

initialPose - The starting pose estimate.

DifferentialDrivePoseEstimator

public DifferentialDrivePoseEstimator(Rotation2d gyroAngle,
 double leftDistance,
 double rightDistance,
 Pose2d initialPose,
 Matrix<N3,N1> stateStdDevs,
 Matrix<N3,N1> visionMeasurementStdDevs)

Constructs a DifferentialDrivePoseEstimator.

Parameters:

gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.

leftDistance - The distance traveled by the left encoder in meters.

rightDistance - The distance traveled by the right encoder in meters.

initialPose - The estimated initial pose.

stateStdDevs - Standard deviations of the pose estimate (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust your state estimate less.

visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.

Method Details

resetPosition

public void resetPosition(Rotation2d gyroAngle,
 double leftPosition,
 double rightPosition,
 Pose2d pose)

Resets the robot's position on the field.

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:

gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.

leftPosition - The distance traveled by the left encoder in meters.

rightPosition - The distance traveled by the right encoder in meters.

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

update

public Pose2d update(Rotation2d gyroAngle,
 double distanceLeft,
 double distanceRight)

Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.

Parameters:

gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.

distanceLeft - The total distance travelled by the left wheel in meters.

distanceRight - The total distance travelled by the right wheel in meters.

Returns:

The estimated pose of the robot in meters.

updateWithTime

public Pose2d updateWithTime(double currentTime,
 Rotation2d gyroAngle,
 double distanceLeft,
 double distanceRight)

Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.

Parameters:

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

gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.

distanceLeft - The total distance travelled by the left wheel in meters.

distanceRight - The total distance travelled by the right wheel in meters.

Returns:

The estimated pose of the robot in meters.