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