edu.wpi.first.math.estimator.SwerveDrivePoseEstimator Class Reference

Public Member Functions
	SwerveDrivePoseEstimator (SwerveDriveKinematics kinematics, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d initialPoseMeters)
	SwerveDrivePoseEstimator (SwerveDriveKinematics kinematics, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d initialPoseMeters, Matrix< N3, N1 > stateStdDevs, Matrix< N3, N1 > visionMeasurementStdDevs)
void	resetPosition (Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters)
Pose2d	update (Rotation2d gyroAngle, SwerveModulePosition[] modulePositions)
Pose2d	updateWithTime (double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions)

Detailed Description

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 replacement for edu.wpi.first.math.kinematics.SwerveDriveOdometry.

SwerveDrivePoseEstimator#update should be called every robot loop.

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

Constructor & Destructor Documentation

SwerveDrivePoseEstimator() [1/2]

edu.wpi.first.math.estimator.SwerveDrivePoseEstimator.SwerveDrivePoseEstimator	(	SwerveDriveKinematics	kinematics,
		Rotation2d	gyroAngle,
		SwerveModulePosition[]	modulePositions,
		Pose2d	initialPoseMeters )

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 measurements and rotations of the swerve modules.
initialPoseMeters	The starting pose estimate.

SwerveDrivePoseEstimator() [2/2]

edu.wpi.first.math.estimator.SwerveDrivePoseEstimator.SwerveDrivePoseEstimator	(	SwerveDriveKinematics	kinematics,
		Rotation2d	gyroAngle,
		SwerveModulePosition[]	modulePositions,
		Pose2d	initialPoseMeters,
		Matrix< N3, N1 >	stateStdDevs,
		Matrix< N3, N1 >	visionMeasurementStdDevs )

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

Member Function Documentation

resetPosition()

void edu.wpi.first.math.estimator.SwerveDrivePoseEstimator.resetPosition	(	Rotation2d	gyroAngle,
		SwerveModulePosition[]	modulePositions,
		Pose2d	poseMeters )

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 measurements and rotations of the swerve modules.
poseMeters	The position on the field that your robot is at.

update()

Pose2d edu.wpi.first.math.estimator.SwerveDrivePoseEstimator.update	(	Rotation2d	gyroAngle,
		SwerveModulePosition[]	modulePositions )

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 measurements and rotations of the swerve modules.

Returns

The estimated pose of the robot in meters.

updateWithTime()

Pose2d edu.wpi.first.math.estimator.SwerveDrivePoseEstimator.updateWithTime	(	double	currentTimeSeconds,
		Rotation2d	gyroAngle,
		SwerveModulePosition[]	modulePositions )

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

Parameters

currentTimeSeconds	Time at which this method was called, in seconds.
gyroAngle	The current gyroscope angle.
modulePositions	The current distance measurements and rotations of the swerve modules.

Returns

The estimated pose of the robot in meters.

---

The documentation for this class was generated from the following file:

• D:/SDASVN/2024/RaspberryPi/VSCode/Source/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java