Class TwoDeadWheelPoseEstimator3d

java.lang.Object
org.wpilib.math.estimator.TwoDeadWheelPoseEstimator3d
public class TwoDeadWheelPoseEstimator3d extends Object
This class wraps Two Dead Wheel Odometry to fuse latency-compensated vision measurements with two dead wheel encoder distance measurements. It is intended to be a drop-in replacement for TwoDeadWheelOdometry3d. It is also intended to be an easy replacement for TwoDeadWheelPoseEstimator, 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(double, double, Rotation3d) should be called every robot loop.

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

    • TwoDeadWheelPoseEstimator3d

      public TwoDeadWheelPoseEstimator3d(double xWheelYPos, double yWheelXPos, double xWheelPos, double yWheelPos, Rotation3d gyroAngle, Pose3d initialPose)
      Constructs a TwoDeadWheelPoseEstimator3d.

      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:
      xWheelYPos - The y-position of the forward-facing wheel relative to the center of the robot in meters.
      yWheelXPos - The x-position of the left-facing wheel relative to the center of the robot in meters.
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The current gyro angle.
      initialPose - The starting pose estimate.

    • TwoDeadWheelPoseEstimator3d

      public TwoDeadWheelPoseEstimator3d(double xWheelYPos, double yWheelXPos, double xWheelPos, double yWheelPos, Rotation3d gyroAngle, Pose3d initialPose, Matrix<N4,N1> stateStdDevs, Matrix<N4,N1> visionMeasurementStdDevs)
      Constructs a TwoDeadWheelPoseEstimator3d.
      Parameters:
      xWheelYPos - The y-position of the forward-facing wheel relative to the center of the robot in meters.
      yWheelXPos - The x-position of the left-facing wheel relative to the center of the robot in meters.
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The current gyro angle.
      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

    • setVisionMeasurementStdDevs

      public final void setVisionMeasurementStdDevs(Matrix<N4,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, z, theta]ᵀ, with units in meters and radians.

    • resetPosition

      public void resetPosition(double xWheelPos, double yWheelPos, Rotation3d gyroAngle, Pose3d pose)
      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:
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The angle reported by the gyroscope.
      pose - The position on the field that your robot is at.

    • resetPose

      public void resetPose(Pose3d pose)
      Resets the robot's pose.
      Parameters:
      pose - The pose to reset to.

    • resetTranslation

      public void resetTranslation(Translation3d translation)
      Resets the robot's translation.
      Parameters:
      translation - The pose to translation to.

    • resetRotation

      public void resetRotation(Rotation3d rotation)
      Resets the robot's rotation.
      Parameters:
      rotation - The rotation to reset to.

    • getEstimatedPosition

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

    • sampleAt

      public Optional<Pose3d> sampleAt(double timestamp)
      Return the pose at a given timestamp, if the buffer is not empty.
      Parameters:
      timestamp - The pose's timestamp in seconds.
      Returns:
      The pose at the given timestamp (or Optional.empty() if the buffer is empty).

    • addVisionMeasurement

      public void addVisionMeasurement(Pose3d visionRobotPose, double timestamp)
      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 PoseEstimator3d.update(Rotation3d, T) 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:
      visionRobotPose - The pose of the robot as measured by the vision camera.
      timestamp - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling PoseEstimator3d.updateWithTime(double,Rotation3d,Object) then you must use a timestamp with the same epoch as Timer.getMonotonicTimestamp().) This means that you should use Timer.getMonotonicTimestamp() as your time source or sync the epochs.

    • addVisionMeasurement

      public void addVisionMeasurement(Pose3d visionRobotPose, double timestamp, Matrix<N4,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 PoseEstimator3d.update(Rotation3d, T) 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 PoseEstimator3d.setVisionMeasurementStdDevs(Matrix) or this method.

      Parameters:
      visionRobotPose - The pose of the robot as measured by the vision camera.
      timestamp - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling updateWithTime(double, double, double, Rotation3d), then you must use a timestamp with the same epoch as Timer.getMonotonicTimestamp()). This means that you should use Timer.getMonotonicTimestamp() as your time source in this case.
      visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y position in meters, z position in meters, and angle in radians). Increase these numbers to trust the vision pose measurement less.

    • update

      public Pose3d update(double xWheelPos, double yWheelPos, Rotation3d gyroAngle)
      Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
      Parameters:
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The current gyro angle.
      Returns:
      The estimated pose of the robot in meters.

    • updateWithTime

      public Pose3d updateWithTime(double currentTime, double xWheelPos, double yWheelPos, Rotation3d gyroAngle)
      Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
      Parameters:
      currentTime - Time at which this method was called, in seconds.
      xWheelPos - The distance traveled by the forward-facing wheel, in meters.
      yWheelPos - The distance traveled by the left-facing wheel, in meters.
      gyroAngle - The current gyro angle.
      Returns:
      The estimated pose of the robot in meters.