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