Class SwerveDrivePoseEstimator3d

java.lang.Object
org.wpilib.math.estimator.PoseEstimator3d<SwerveModulePosition[]>
org.wpilib.math.estimator.SwerveDrivePoseEstimator3d
public class SwerveDrivePoseEstimator3d extends PoseEstimator3d<SwerveModulePosition[]>
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 SwerveDriveOdometry3d. It is also intended to be an easy replacement for SwerveDrivePoseEstimator, 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().)

PoseEstimator3d.update(Rotation3d, T) should be called every robot loop.

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

  • Constructor Details

    • SwerveDrivePoseEstimator3d

      public SwerveDrivePoseEstimator3d(SwerveDriveKinematics kinematics, Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d initialPose)
      Constructs a SwerveDrivePoseEstimator3d 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, 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision measurements are 0.9 meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for angle.

      Parameters:
      kinematics - A correctly-configured kinematics object for your drivetrain.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.
      modulePositions - The current distance measurements and rotations of the swerve modules.
      initialPose - The starting pose estimate.

    • SwerveDrivePoseEstimator3d

      public SwerveDrivePoseEstimator3d(SwerveDriveKinematics kinematics, Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d initialPose, Matrix<N4,N1> stateStdDevs, Matrix<N4,N1> visionMeasurementStdDevs)
      Constructs a SwerveDrivePoseEstimator3d.
      Parameters:
      kinematics - A correctly-configured kinematics object for your drivetrain.
      gyroAngle - The angle reported by the gyroscope. This does not need to be offset to match the robot's orientation on the field.
      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.

  • Method Details

    • updateWithTime

      public Pose3d updateWithTime(double currentTime, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions)
      Description copied from class: PoseEstimator3d
      Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
      Overrides:
      updateWithTime in class PoseEstimator3d<SwerveModulePosition[]>
      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.
      wheelPositions - The current encoder readings.
      Returns:
      The estimated pose of the robot in meters.