Package edu.wpi.first.math.estimator

Class DifferentialDrivePoseEstimator

java.lang.Object
edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator
public class DifferentialDrivePoseEstimator
extends Object
This class wraps Differential Drive Odometry to fuse latency-compensated vision measurements with differential drive encoder measurements. It is intended to be a drop-in replacement for DifferentialDriveOdometry; in fact, if you never call addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d, double) and only call update(edu.wpi.first.math.geometry.Rotation2d, double, double) then this will behave exactly the same as DifferentialDriveOdometry.

update(edu.wpi.first.math.geometry.Rotation2d, double, double) should be called every robot loop.

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 exactly like regular encoder odometry.

  • Constructor Details

    • DifferentialDrivePoseEstimator

      public DifferentialDrivePoseEstimator​(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters, Pose2d initialPoseMeters)
      Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and vision measurements.

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

      Parameters:
      kinematics - A correctly-configured kinematics object for your drivetrain.
      gyroAngle - The current gyro angle.
      leftDistanceMeters - The distance traveled by the left encoder.
      rightDistanceMeters - The distance traveled by the right encoder.
      initialPoseMeters - The starting pose estimate.

    • DifferentialDrivePoseEstimator

      public DifferentialDrivePoseEstimator​(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters, Pose2d initialPoseMeters, Matrix<N3,​N1> stateStdDevs, Matrix<N3,​N1> visionMeasurementStdDevs)
      Constructs a DifferentialDrivePoseEstimator.
      Parameters:
      kinematics - A correctly-configured kinematics object for your drivetrain.
      gyroAngle - The gyro angle of the robot.
      leftDistanceMeters - The distance traveled by the left encoder.
      rightDistanceMeters - The distance traveled by the right encoder.
      initialPoseMeters - The estimated initial pose.
      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

    • 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​(Rotation2d gyroAngle, double leftPositionMeters, double rightPositionMeters, Pose2d poseMeters)
      Resets the robot's position on the field.

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

      Parameters:
      gyroAngle - The angle reported by the gyroscope.
      leftPositionMeters - The distance traveled by the left encoder.
      rightPositionMeters - The distance traveled by the right encoder.
      poseMeters - The position on the field that your robot is at.

    • getEstimatedPosition

      public Pose2d getEstimatedPosition()
      Gets the estimated robot pose.
      Returns:
      The estimated robot pose in meters.

    • addVisionMeasurement

      public void addVisionMeasurement​(Pose2d visionRobotPoseMeters, double timestampSeconds)
      Adds a vision measurement to the 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, double, double) every loop.

      To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.

      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,Rotation2d,double,double) 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)
      Adds a vision measurement to the 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, double, double) every loop.

      To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.

      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,Rotation2d,double,double), 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 pose measurement (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.

    • update

      public Pose2d update​(Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters)
      Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
      Parameters:
      gyroAngle - The current gyro angle.
      distanceLeftMeters - The total distance travelled by the left wheel in meters.
      distanceRightMeters - The total distance travelled by the right wheel in meters.
      Returns:
      The estimated pose of the robot in meters.

    • updateWithTime

      public Pose2d updateWithTime​(double currentTimeSeconds, Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters)
      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 gyro angle.
      distanceLeftMeters - The total distance travelled by the left wheel in meters.
      distanceRightMeters - The total distance travelled by the right wheel in meters.
      Returns:
      The estimated pose of the robot in meters.