DifferentialDrivePoseEstimator3d

class wpimath.DifferentialDrivePoseEstimator3d(*args, **kwargs)

Bases: DifferentialDrivePoseEstimator3dBase

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 DifferentialDriveOdometry3d. In fact, if you never call AddVisionMeasurement(), and only call Update(), this will behave exactly the same as DifferentialDriveOdometry3d. It is also intended to be an easy replacement for PoseEstimator, only requiring the addition of a standard deviation for Z and appropriate conversions between 2D and 3D versions of geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d), Translation3d(Translation2d), and Pose3d.ToPose2d().)

Update() should be called every robot loop (if your robot loops are faster or slower than the default of 20 ms, then you should change the nominal delta time via the constructor).

AddVisionMeasurement() can be called as infrequently as you want; if you never call it, then this class will behave like regular encoder odometry.

Overloaded function.

  1. __init__(self: wpimath._wpimath.DifferentialDrivePoseEstimator3d, kinematics: wpimath._wpimath.DifferentialDriveKinematics, gyroAngle: wpimath._wpimath.Rotation3d, leftDistance: wpimath.units.meters, rightDistance: wpimath.units.meters, initialPose: wpimath._wpimath.Pose3d) -> None

Constructs a DifferentialDrivePoseEstimator3d 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, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for angle.

Parameters:

  • kinematics – A correctly-configured kinematics object for your drivetrain.

  • gyroAngle – The gyro angle of the robot.

  • leftDistance – The distance traveled by the left encoder.

  • rightDistance – The distance traveled by the right encoder.

  • initialPose – The estimated initial pose.

  1. __init__(self: wpimath._wpimath.DifferentialDrivePoseEstimator3d, kinematics: wpimath._wpimath.DifferentialDriveKinematics, gyroAngle: wpimath._wpimath.Rotation3d, leftDistance: wpimath.units.meters, rightDistance: wpimath.units.meters, initialPose: wpimath._wpimath.Pose3d, stateStdDevs: Tuple[typing.SupportsFloat | typing.SupportsIndex, typing.SupportsFloat | typing.SupportsIndex, typing.SupportsFloat | typing.SupportsIndex, typing.SupportsFloat | typing.SupportsIndex], visionMeasurementStdDevs: Tuple[typing.SupportsFloat | typing.SupportsIndex, typing.SupportsFloat | typing.SupportsIndex, typing.SupportsFloat | typing.SupportsIndex, typing.SupportsFloat | typing.SupportsIndex]) -> None

Constructs a DifferentialDrivePoseEstimator3d.

Parameters:

  • kinematics – A correctly-configured kinematics object for your drivetrain.

  • gyroAngle – The gyro angle of the robot.

  • leftDistance – The distance traveled by the left encoder.

  • rightDistance – The distance traveled by the right encoder.

  • initialPose – The estimated initial pose.

  • stateStdDevs – Standard deviations of the pose estimate (x position in meters, y position in meters, z position in meters, and angle 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, z position in meters, and angle in radians). Increase these numbers to trust the vision pose measurement less.

resetPosition(gyroAngle: wpimath._wpimath.Rotation3d, leftDistance: wpimath.units.meters, rightDistance: wpimath.units.meters, pose: wpimath._wpimath.Pose3d) → None

Resets the robot’s position on the field.

Parameters:

  • gyroAngle – The current gyro angle.

  • leftDistance – The distance traveled by the left encoder.

  • rightDistance – The distance traveled by the right encoder.

  • pose – The estimated pose of the robot on the field.

update(gyroAngle: wpimath._wpimath.Rotation3d, leftDistance: wpimath.units.meters, rightDistance: wpimath.units.meters) → wpimath._wpimath.Pose3d

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

Parameters:

  • gyroAngle – The current gyro angle.

  • leftDistance – The distance traveled by the left encoder.

  • rightDistance – The distance traveled by the right encoder.

Returns:

The estimated pose of the robot.

updateWithTime(currentTime: wpimath.units.seconds, gyroAngle: wpimath._wpimath.Rotation3d, leftDistance: wpimath.units.meters, rightDistance: wpimath.units.meters) → wpimath._wpimath.Pose3d

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

Parameters:

  • currentTime – The time at which this method was called.

  • gyroAngle – The current gyro angle.

  • leftDistance – The distance traveled by the left encoder.

  • rightDistance – The distance traveled by the right encoder.

Returns:

The estimated pose of the robot.