Package swervelib.math

Class SwervePoseEstimator2

java.lang.Object
swervelib.math.SwervePoseEstimator2
public class SwervePoseEstimator2 extends Object
Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more accurate estimation, originally made by Team 1466.

  • Nested Class Summary

    Nested Classes
    Modifier and Type
    Class
    Description
    private class 
    SwervePoseEstimator2.InterpolationRecord
    Represents an odometry record.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private final edu.wpi.first.math.kinematics.SwerveDriveKinematics
    m_kinematics
    Swerve drive kinematics.
    private final int
    m_numModules
    Number of swerve modules.
    private final SwerveDriveOdometry2
    m_odometry
    Enhanced swerve drive odometry.
    private final edu.wpi.first.math.interpolation.TimeInterpolatableBuffer<SwervePoseEstimator2.InterpolationRecord>
    m_poseBuffer
    Interpolation buffer.
    private final edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1>
    m_q
    Matrix quotient.
    private final edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N3>
    m_visionK
    Vision standard deviations.

  • Constructor Summary

    Constructors
    Constructor
    Description
    SwervePoseEstimator2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d initialPoseMeters)
    Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.
    SwervePoseEstimator2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d initialPoseMeters, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> stateStdDevs, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
    Constructs a SwerveDrivePoseEstimator.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d visionRobotPoseMeters, double timestampSeconds)
    Adds a vision measurement to the Kalman Filter.
    void
    addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d visionRobotPoseMeters, double timestampSeconds, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
    Adds a vision measurement to the Kalman Filter.
    edu.wpi.first.math.geometry.Pose2d
    getEstimatedPosition()
    Gets the estimated robot pose.
    void
    resetPosition(edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d poseMeters)
    Resets the robot's position on the field.
    void
    setVisionMeasurementStdDevs(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
    Sets the pose estimator's trust of global measurements.
    edu.wpi.first.math.geometry.Pose2d
    update(edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.geometry.Rotation2d gyroPitch, edu.wpi.first.math.geometry.Rotation2d gyroRoll, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
    Updates the pose estimator with wheel encoder and gyro information.
    edu.wpi.first.math.geometry.Pose2d
    updateWithTime(double currentTimeSeconds, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.geometry.Rotation2d gyroPitch, edu.wpi.first.math.geometry.Rotation2d gyroRoll, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
    Updates the pose estimator with wheel encoder and gyro information.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Field Details

    • m_kinematics

      private final edu.wpi.first.math.kinematics.SwerveDriveKinematics m_kinematics
      Swerve drive kinematics.

    • m_odometry

      private final SwerveDriveOdometry2 m_odometry
      Enhanced swerve drive odometry.

    • m_q

      private final edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> m_q
      Matrix quotient.

    • m_numModules

      private final int m_numModules
      Number of swerve modules.

    • m_poseBuffer

      private final edu.wpi.first.math.interpolation.TimeInterpolatableBuffer<SwervePoseEstimator2.InterpolationRecord> m_poseBuffer
      Interpolation buffer.

    • m_visionK

      private final edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N3> m_visionK
      Vision standard deviations.

  • Constructor Details

    • SwervePoseEstimator2

      public SwervePoseEstimator2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d initialPoseMeters)
      Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.

      The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.

      Parameters:
      kinematics - A correctly-configured kinematics object for your drivetrain.
      gyroAngle - The current gyro angle.
      modulePositions - The current distance measurements and rotations of the swerve modules.
      initialPoseMeters - The starting pose estimate.

    • SwervePoseEstimator2

      public SwervePoseEstimator2(edu.wpi.first.math.kinematics.SwerveDriveKinematics kinematics, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d initialPoseMeters, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> stateStdDevs, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
      Constructs a SwerveDrivePoseEstimator.
      Parameters:
      kinematics - A correctly-configured kinematics object for your drivetrain.
      gyroAngle - The current gyro angle.
      modulePositions - The current distance and rotation measurements of the swerve modules.
      initialPoseMeters - The starting pose estimate.
      stateStdDevs - Standard deviations of the pose estimate (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust your state estimate less.
      visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.

  • Method Details

    • setVisionMeasurementStdDevs

      public void setVisionMeasurementStdDevs(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
      Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases.
      Parameters:
      visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta]^T, with units in meters and radians.

    • resetPosition

      public void resetPosition(edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions, edu.wpi.first.math.geometry.Pose2d poseMeters)
      Resets the robot's position on the field.

      The gyroscope angle does not need to be reset in the user's robot code. The library automatically takes care of offsetting the gyro angle.

      Parameters:
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The current distance measurements and rotations of the swerve modules.
      poseMeters - The position on the field that your robot is at.

    • getEstimatedPosition

      public edu.wpi.first.math.geometry.Pose2d getEstimatedPosition()
      Gets the estimated robot pose.
      Returns:
      The estimated robot pose in meters.

    • addVisionMeasurement

      public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d visionRobotPoseMeters, double timestampSeconds)
      Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting for measurement noise.

      This method can be called as infrequently as you want, as long as you are calling SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) every loop.

      To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.

      Parameters:
      visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
      timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[]) then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as Timer.getFPGATimestamp().) This means that you should use Timer.getFPGATimestamp() as your time source or sync the epochs.

    • addVisionMeasurement

      public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d visionRobotPoseMeters, double timestampSeconds, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
      Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting for measurement noise.

      This method can be called as infrequently as you want, as long as you are calling SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) every loop.

      To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.

      Note that the vision measurement standard deviations passed into this method will continue to apply to future measurements until a subsequent call to SwerveDrivePoseEstimator.setVisionMeasurementStdDevs(Matrix) or this method.

      Parameters:
      visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
      timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[]), then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as Timer.getFPGATimestamp()). This means that you should use Timer.getFPGATimestamp() as your time source in this case.
      visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.

    • update

      public edu.wpi.first.math.geometry.Pose2d update(edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.geometry.Rotation2d gyroPitch, edu.wpi.first.math.geometry.Rotation2d gyroRoll, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
      Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
      Parameters:
      gyroAngle - The current gyro angle.
      gyroPitch - The current gyro pitch.
      gyroRoll - The current gyro roll.
      modulePositions - The current distance measurements and rotations of the swerve modules.
      Returns:
      The estimated pose of the robot in meters.

    • updateWithTime

      public edu.wpi.first.math.geometry.Pose2d updateWithTime(double currentTimeSeconds, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.geometry.Rotation2d gyroPitch, edu.wpi.first.math.geometry.Rotation2d gyroRoll, edu.wpi.first.math.kinematics.SwerveModulePosition[] modulePositions)
      Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
      Parameters:
      currentTimeSeconds - Time at which this method was called, in seconds.
      gyroAngle - The current gyro angle.
      gyroPitch - The current gyro pitch.
      gyroRoll - The current gyro roll.
      modulePositions - The current distance measurements and rotations of the swerve modules.
      Returns:
      The estimated pose of the robot in meters.