Package edu.wpi.first.math.estimator

Class DifferentialDrivePoseEstimator3d

java.lang.Object
edu.wpi.first.math.estimator.PoseEstimator3d<DifferentialDriveWheelPositions>
edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator3d
public class DifferentialDrivePoseEstimator3d extends PoseEstimator3d<DifferentialDriveWheelPositions>
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 DifferentialDriveOdometry3d; in fact, if you never call PoseEstimator3d.addVisionMeasurement(edu.wpi.first.math.geometry.Pose3d, double) and only call update(edu.wpi.first.math.geometry.Rotation3d, double, double) then this will behave exactly the same as DifferentialDriveOdometry3d. It is also intended to be an easy replacement for DifferentialDrivePoseEstimator, 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().)

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

PoseEstimator3d.addVisionMeasurement(edu.wpi.first.math.geometry.Pose3d, 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

    • DifferentialDrivePoseEstimator3d

      public DifferentialDrivePoseEstimator3d(DifferentialDriveKinematics kinematics, Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters, Pose3d initialPoseMeters)
      Constructs a DifferentialDrivePoseEstimator3d 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, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for angle.

      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.

    • DifferentialDrivePoseEstimator3d

      public DifferentialDrivePoseEstimator3d(DifferentialDriveKinematics kinematics, Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters, Pose3d initialPoseMeters, Matrix<N4,N1> stateStdDevs, Matrix<N4,N1> visionMeasurementStdDevs)
      Constructs a DifferentialDrivePoseEstimator3d.
      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

    • resetPosition

      public void resetPosition(Rotation3d gyroAngle, double leftPositionMeters, double rightPositionMeters, Pose3d 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.

    • update

      public Pose3d update(Rotation3d 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 Pose3d updateWithTime(double currentTimeSeconds, Rotation3d 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.