001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.estimator;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.VecBuilder;
009import edu.wpi.first.math.geometry.Pose2d;
010import edu.wpi.first.math.geometry.Pose3d;
011import edu.wpi.first.math.geometry.Rotation2d;
012import edu.wpi.first.math.geometry.Rotation3d;
013import edu.wpi.first.math.geometry.Translation2d;
014import edu.wpi.first.math.geometry.Translation3d;
015import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
016import edu.wpi.first.math.kinematics.DifferentialDriveOdometry3d;
017import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
018import edu.wpi.first.math.numbers.N1;
019import edu.wpi.first.math.numbers.N4;
020
021/**
022 * This class wraps {@link DifferentialDriveOdometry3d Differential Drive Odometry} to fuse
023 * latency-compensated vision measurements with differential drive encoder measurements. It is
024 * intended to be a drop-in replacement for {@link DifferentialDriveOdometry3d}; in fact, if you
025 * never call {@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} and only call {@link
026 * DifferentialDrivePoseEstimator3d#update} then this will behave exactly the same as
027 * DifferentialDriveOdometry3d. It is also intended to be an easy replacement for {@link
028 * DifferentialDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and
029 * appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
030 * Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
031 * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
032 *
033 * <p>{@link DifferentialDrivePoseEstimator3d#update} should be called every robot loop.
034 *
035 * <p>{@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as
036 * you want; if you never call it then this class will behave exactly like regular encoder odometry.
037 */
038public class DifferentialDrivePoseEstimator3d
039    extends PoseEstimator3d<DifferentialDriveWheelPositions> {
040  /**
041   * Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model
042   * and vision measurements.
043   *
044   * <p>The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for
045   * y, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision
046   * measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for
047   * angle.
048   *
049   * @param kinematics A correctly-configured kinematics object for your drivetrain.
050   * @param gyroAngle The current gyro angle.
051   * @param leftDistance The distance traveled by the left encoder in meters.
052   * @param rightDistance The distance traveled by the right encoder in meters.
053   * @param initialPose The starting pose estimate.
054   */
055  public DifferentialDrivePoseEstimator3d(
056      DifferentialDriveKinematics kinematics,
057      Rotation3d gyroAngle,
058      double leftDistance,
059      double rightDistance,
060      Pose3d initialPose) {
061    this(
062        kinematics,
063        gyroAngle,
064        leftDistance,
065        rightDistance,
066        initialPose,
067        VecBuilder.fill(0.02, 0.02, 0.02, 0.01),
068        VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
069  }
070
071  /**
072   * Constructs a DifferentialDrivePoseEstimator3d.
073   *
074   * @param kinematics A correctly-configured kinematics object for your drivetrain.
075   * @param gyroAngle The gyro angle of the robot.
076   * @param leftDistance The distance traveled by the left encoder in meters.
077   * @param rightDistance The distance traveled by the right encoder in meters.
078   * @param initialPose The estimated initial pose.
079   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
080   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
081   *     less.
082   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
083   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
084   *     the vision pose measurement less.
085   */
086  public DifferentialDrivePoseEstimator3d(
087      DifferentialDriveKinematics kinematics,
088      Rotation3d gyroAngle,
089      double leftDistance,
090      double rightDistance,
091      Pose3d initialPose,
092      Matrix<N4, N1> stateStdDevs,
093      Matrix<N4, N1> visionMeasurementStdDevs) {
094    super(
095        kinematics,
096        new DifferentialDriveOdometry3d(gyroAngle, leftDistance, rightDistance, initialPose),
097        stateStdDevs,
098        visionMeasurementStdDevs);
099  }
100
101  /**
102   * Resets the robot's position on the field.
103   *
104   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
105   * automatically takes care of offsetting the gyro angle.
106   *
107   * @param gyroAngle The angle reported by the gyroscope.
108   * @param leftPosition The distance traveled by the left encoder in meters.
109   * @param rightPosition The distance traveled by the right encoder in meters.
110   * @param pose The position on the field that your robot is at.
111   */
112  public void resetPosition(
113      Rotation3d gyroAngle, double leftPosition, double rightPosition, Pose3d pose) {
114    resetPosition(
115        gyroAngle, new DifferentialDriveWheelPositions(leftPosition, rightPosition), pose);
116  }
117
118  /**
119   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
120   * loop.
121   *
122   * @param gyroAngle The current gyro angle.
123   * @param distanceLeft The total distance travelled by the left wheel in meters.
124   * @param distanceRight The total distance travelled by the right wheel in meters.
125   * @return The estimated pose of the robot in meters.
126   */
127  public Pose3d update(Rotation3d gyroAngle, double distanceLeft, double distanceRight) {
128    return update(gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
129  }
130
131  /**
132   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
133   * loop.
134   *
135   * @param currentTime Time at which this method was called, in seconds.
136   * @param gyroAngle The current gyro angle.
137   * @param distanceLeft The total distance travelled by the left wheel in meters.
138   * @param distanceRight The total distance travelled by the right wheel in meters.
139   * @return The estimated pose of the robot in meters.
140   */
141  public Pose3d updateWithTime(
142      double currentTime, Rotation3d gyroAngle, double distanceLeft, double distanceRight) {
143    return updateWithTime(
144        currentTime, gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
145  }
146}