MecanumDrivePoseEstimator

class wpimath.estimator.MecanumDrivePoseEstimator(*args, **kwargs)

Bases: MecanumDrivePoseEstimatorBase

This class wraps an 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 MecanumDriveOdometry.

update() should be called every robot loop. If your loops are faster or slower than the default of 0.02s, then you should change the nominal delta time by specifying it in the constructor.

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):

\(x = [x, y, \theta]^T\) in the field-coordinate system containing x position, y position, and heading.

\(u = [v_x, v_y, \omega]^T\) containing x velocity, y velocity, and angular velocity in the field-coordinate system.

\(y = [x, y, \theta]^T\) from vision containing x position, y position, and heading; or \(y = [theta]^T\) containing gyro heading.

Overloaded function.

  1. __init__(self: wpimath._controls._controls.estimator.MecanumDrivePoseEstimator, kinematics: wpimath.kinematics._kinematics.MecanumDriveKinematics, gyroAngle: wpimath.geometry._geometry.Rotation2d, wheelPositions: wpimath.kinematics._kinematics.MecanumDriveWheelPositions, initialPose: wpimath.geometry._geometry.Pose2d) -> None

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

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

Parameters:

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

  • gyroAngle – The current gyro angle.

  • wheelPositions – The distance measured by each wheel.

  • initialPose – The starting pose estimate.

  1. __init__(self: wpimath._controls._controls.estimator.MecanumDrivePoseEstimator, kinematics: wpimath.kinematics._kinematics.MecanumDriveKinematics, gyroAngle: wpimath.geometry._geometry.Rotation2d, wheelPositions: wpimath.kinematics._kinematics.MecanumDriveWheelPositions, initialPose: wpimath.geometry._geometry.Pose2d, stateStdDevs: Tuple[float, float, float], visionMeasurementStdDevs: Tuple[float, float, float]) -> None

Constructs a MecanumDrivePoseEstimator.

Parameters:

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

  • gyroAngle – The current gyro angle.

  • wheelPositions – The distance measured by each wheel.

  • initialPose – The starting pose estimate.

  • 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.