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.SwerveDriveKinematics;
012import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
013import edu.wpi.first.math.kinematics.SwerveModulePosition;
014import edu.wpi.first.math.numbers.N1;
015import edu.wpi.first.math.numbers.N3;
016
017/**
018 * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
019 * vision measurements with swerve drive encoder distance measurements. It is intended to be a
020 * drop-in replacement for {@link SwerveDriveOdometry}.
021 *
022 * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
023 *
024 * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
025 * want; if you never call it, then this class will behave as regular encoder odometry.
026 */
027public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveModulePosition[]> {
028  private final int m_numModules;
029
030  /**
031   * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
032   * measurements.
033   *
034   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
035   * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9
036   * meters for x, 0.9 meters for y, and 0.9 radians for heading.
037   *
038   * @param kinematics A correctly-configured kinematics object for your drivetrain.
039   * @param gyroAngle The current gyro angle.
040   * @param modulePositions The current distance measurements and rotations of the swerve modules.
041   * @param initialPoseMeters The starting pose estimate.
042   */
043  public SwerveDrivePoseEstimator(
044      SwerveDriveKinematics kinematics,
045      Rotation2d gyroAngle,
046      SwerveModulePosition[] modulePositions,
047      Pose2d initialPoseMeters) {
048    this(
049        kinematics,
050        gyroAngle,
051        modulePositions,
052        initialPoseMeters,
053        VecBuilder.fill(0.1, 0.1, 0.1),
054        VecBuilder.fill(0.9, 0.9, 0.9));
055  }
056
057  /**
058   * Constructs a SwerveDrivePoseEstimator.
059   *
060   * @param kinematics A correctly-configured kinematics object for your drivetrain.
061   * @param gyroAngle The current gyro angle.
062   * @param modulePositions The current distance and rotation measurements of the swerve modules.
063   * @param initialPoseMeters The starting pose estimate.
064   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
065   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
066   *     less.
067   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
068   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
069   *     the vision pose measurement less.
070   */
071  public SwerveDrivePoseEstimator(
072      SwerveDriveKinematics kinematics,
073      Rotation2d gyroAngle,
074      SwerveModulePosition[] modulePositions,
075      Pose2d initialPoseMeters,
076      Matrix<N3, N1> stateStdDevs,
077      Matrix<N3, N1> visionMeasurementStdDevs) {
078    super(
079        kinematics,
080        new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
081        stateStdDevs,
082        visionMeasurementStdDevs);
083
084    m_numModules = modulePositions.length;
085  }
086
087  @Override
088  public Pose2d updateWithTime(
089      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) {
090    if (wheelPositions.length != m_numModules) {
091      throw new IllegalArgumentException(
092          "Number of modules is not consistent with number of wheel locations provided in "
093              + "constructor");
094    }
095
096    return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
097  }
098}