Package edu.wpi.first.math.estimator

Class SwerveDrivePoseEstimator

java.lang.Object
edu.wpi.first.math.estimator.SwerveDrivePoseEstimator
public class SwerveDrivePoseEstimator
extends Object
This class wraps an Unscented Kalman Filter to fuse latency-compensated vision measurements with swerve 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 SwerveDriveOdometry.

update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModuleState...) 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 using the secondary constructor: SwerveDrivePoseEstimator(Rotation2d, Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix, double).

addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d, double) 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]ᵀ in the field coordinate system containing x position, y position, and heading.

u = [v_l, v_r, dtheta]ᵀ containing left wheel velocity, right wheel velocity, and change in gyro heading.

y = [x, y, theta]ᵀ from vision containing x position, y position, and heading; or y = [theta]ᵀ containing gyro heading.

  • Constructor Details

    • SwerveDrivePoseEstimator

      public SwerveDrivePoseEstimator​(Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics, Matrix<N3,​N1> stateStdDevs, Matrix<N1,​N1> localMeasurementStdDevs, Matrix<N3,​N1> visionMeasurementStdDevs)
      Constructs a SwerveDrivePoseEstimator.
      Parameters:
      gyroAngle - The current gyro angle.
      initialPoseMeters - The starting pose estimate.
      kinematics - A correctly-configured kinematics object for your drivetrain.
      stateStdDevs - Standard deviations of model states. Increase these numbers to trust your model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
      localMeasurementStdDevs - Standard deviations of the encoder and gyro measurements. Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is in the form [theta], with units in radians.
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.

    • SwerveDrivePoseEstimator

      public SwerveDrivePoseEstimator​(Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics, Matrix<N3,​N1> stateStdDevs, Matrix<N1,​N1> localMeasurementStdDevs, Matrix<N3,​N1> visionMeasurementStdDevs, double nominalDtSeconds)
      Constructs a SwerveDrivePoseEstimator.
      Parameters:
      gyroAngle - The current gyro angle.
      initialPoseMeters - The starting pose estimate.
      kinematics - A correctly-configured kinematics object for your drivetrain.
      stateStdDevs - Standard deviations of model states. Increase these numbers to trust your model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
      localMeasurementStdDevs - Standard deviations of the encoder and gyro measurements. Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is in the form [theta], with units in radians.
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
      nominalDtSeconds - The time in seconds between each robot loop.

  • Method Details

    • setVisionMeasurementStdDevs

      public void setVisionMeasurementStdDevs​(Matrix<N3,​N1> visionMeasurementStdDevs)
      Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases.
      Parameters:
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.

    • resetPosition

      public void resetPosition​(Pose2d poseMeters, Rotation2d gyroAngle)
      Resets the robot's position on the field.

      You NEED to reset your encoders (to zero) when calling this method.

      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:
      poseMeters - The position on the field that your robot is at.
      gyroAngle - The angle reported by the gyroscope.

    • getEstimatedPosition

      public Pose2d getEstimatedPosition()
      Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
      Returns:
      The estimated robot pose in meters.

    • addVisionMeasurement

      public void addVisionMeasurement​(Pose2d visionRobotPoseMeters, double timestampSeconds)
      Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose estimate while still accounting for measurement noise.

      This method can be called as infrequently as you want, as long as you are calling update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModuleState...) every loop.

      Parameters:
      visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
      timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling updateWithTime(double, edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModuleState...) then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time source or sync the epochs.

    • addVisionMeasurement

      public void addVisionMeasurement​(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,​N1> visionMeasurementStdDevs)
      Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose estimate while still accounting for measurement noise.

      This method can be called as infrequently as you want, as long as you are calling update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModuleState...) every loop.

      Note that the vision measurement standard deviations passed into this method will continue to apply to future measurements until a subsequent call to setVisionMeasurementStdDevs(Matrix) or this method.

      Parameters:
      visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
      timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling updateWithTime(double, edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModuleState...) then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time source in this case.
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.

    • update

      public Pose2d update​(Rotation2d gyroAngle, SwerveModuleState... moduleStates)
      Updates the the Unscented Kalman Filter using only wheel encoder information. This should be called every loop, and the correct loop period must be passed into the constructor of this class.
      Parameters:
      gyroAngle - The current gyro angle.
      moduleStates - The current velocities and rotations of the swerve modules.
      Returns:
      The estimated pose of the robot in meters.

    • updateWithTime

      public Pose2d updateWithTime​(double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates)
      Updates the the Unscented Kalman Filter using only wheel encoder information. This should be called every loop, and the correct loop period must be passed into the constructor of this class.
      Parameters:
      currentTimeSeconds - Time at which this method was called, in seconds.
      gyroAngle - The current gyroscope angle.
      moduleStates - The current velocities and rotations of the swerve modules.
      Returns:
      The estimated pose of the robot in meters.