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.MecanumDriveKinematics;
016import edu.wpi.first.math.kinematics.MecanumDriveOdometry3d;
017import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
018import edu.wpi.first.math.numbers.N1;
019import edu.wpi.first.math.numbers.N4;
020
021/**
022 * This class wraps {@link MecanumDriveOdometry3d Mecanum Drive Odometry} to fuse
023 * latency-compensated vision measurements with mecanum drive encoder distance measurements. It will
024 * correct for noisy measurements and encoder drift. It is intended to be a drop-in replacement for
025 * {@link MecanumDriveOdometry3d}. It is also intended to be an easy replacement for {@link
026 * MecanumDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and
027 * appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
028 * Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
029 * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
030 *
031 * <p>{@link MecanumDrivePoseEstimator3d#update} should be called every robot loop.
032 *
033 * <p>{@link MecanumDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as you
034 * want; if you never call it, then this class will behave mostly like regular encoder odometry.
035 */
036public class MecanumDrivePoseEstimator3d extends PoseEstimator3d<MecanumDriveWheelPositions> {
037  /**
038   * Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and
039   * vision measurements.
040   *
041   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
042   * 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision
043   * measurements are 0.45 meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for
044   * angle.
045   *
046   * @param kinematics A correctly-configured kinematics object for your drivetrain.
047   * @param gyroAngle The current gyro angle.
048   * @param wheelPositions The distances driven by each wheel.
049   * @param initialPoseMeters The starting pose estimate.
050   */
051  public MecanumDrivePoseEstimator3d(
052      MecanumDriveKinematics kinematics,
053      Rotation3d gyroAngle,
054      MecanumDriveWheelPositions wheelPositions,
055      Pose3d initialPoseMeters) {
056    this(
057        kinematics,
058        gyroAngle,
059        wheelPositions,
060        initialPoseMeters,
061        VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
062        VecBuilder.fill(0.45, 0.45, 0.45, 0.45));
063  }
064
065  /**
066   * Constructs a MecanumDrivePoseEstimator3d.
067   *
068   * @param kinematics A correctly-configured kinematics object for your drivetrain.
069   * @param gyroAngle The current gyro angle.
070   * @param wheelPositions The distance measured by each wheel.
071   * @param initialPoseMeters The starting pose estimate.
072   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
073   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
074   *     less.
075   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
076   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
077   *     the vision pose measurement less.
078   */
079  public MecanumDrivePoseEstimator3d(
080      MecanumDriveKinematics kinematics,
081      Rotation3d gyroAngle,
082      MecanumDriveWheelPositions wheelPositions,
083      Pose3d initialPoseMeters,
084      Matrix<N4, N1> stateStdDevs,
085      Matrix<N4, N1> visionMeasurementStdDevs) {
086    super(
087        kinematics,
088        new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
089        stateStdDevs,
090        visionMeasurementStdDevs);
091  }
092}