SwerveDrive4PoseEstimator

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

Bases: SwerveDrive4PoseEstimatorBase

This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve drive encoder distance measurements. It is intended to be a drop-in for SwerveDriveOdometry.

update() should be called every robot loop.

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

The state-space system used internally has the following states (x) and outputs (y):

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

\(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.SwerveDrive4PoseEstimator, kinematics: wpimath.kinematics._kinematics.SwerveDrive4Kinematics, gyroAngle: wpimath.geometry._geometry.Rotation2d, modulePositions: Tuple[wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition], initialPose: wpimath.geometry._geometry.Pose2d) -> None

Constructs a SwerveDrivePoseEstimator 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.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.

Parameters:

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

  • gyroAngle – The current gyro angle.

  • modulePositions – The current distance and rotation measurements of the swerve modules.

  • initialPose – The starting pose estimate.

  1. __init__(self: wpimath._controls._controls.estimator.SwerveDrive4PoseEstimator, kinematics: wpimath.kinematics._kinematics.SwerveDrive4Kinematics, gyroAngle: wpimath.geometry._geometry.Rotation2d, modulePositions: Tuple[wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition], initialPose: wpimath.geometry._geometry.Pose2d, stateStdDevs: Tuple[float, float, float], visionMeasurementStdDevs: Tuple[float, float, float]) -> None

Constructs a SwerveDrivePoseEstimator.

Parameters:

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

  • gyroAngle – The current gyro angle.

  • modulePositions – The current distance and rotation measurements of the swerve modules.

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

resetPosition(gyroAngle: wpimath.geometry._geometry.Rotation2d, modulePositions: Tuple[wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition], pose: wpimath.geometry._geometry.Pose2d) → None

Resets the robot’s position on the field.

The gyroscope angle does not need to be reset in the user’s robot code. The library automatically takes care of offsetting the gyro angle.

Parameters:

  • gyroAngle – The angle reported by the gyroscope.

  • modulePositions – The current distance and rotation measurements of the swerve modules.

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

update(gyroAngle: wpimath.geometry._geometry.Rotation2d, modulePositions: Tuple[wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition]) → wpimath.geometry._geometry.Pose2d

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

Parameters:

  • gyroAngle – The current gyro angle.

  • modulePositions – The current distance and rotation measurements of the swerve modules.

Returns:

The estimated robot pose in meters.

updateWithTime(currentTime: wpimath.units.seconds, gyroAngle: wpimath.geometry._geometry.Rotation2d, modulePositions: Tuple[wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition, wpimath.kinematics._kinematics.SwerveModulePosition]) → wpimath.geometry._geometry.Pose2d

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 current gyro angle.

  • modulePositions – The current distance traveled and rotations of the swerve modules.

Returns:

The estimated robot pose in meters.