Class MecanumDrivePoseEstimator

    • Constructor Detail

      • MecanumDrivePoseEstimator

        public MecanumDrivePoseEstimator​(Rotation2d gyroAngle,
                                         Pose2d initialPoseMeters,
                                         MecanumDriveKinematics kinematics,
                                         Matrix<N3,​N1> stateStdDevs,
                                         Matrix<N1,​N1> localMeasurementStdDevs,
                                         Matrix<N3,​N1> visionMeasurementStdDevs)
        Constructs a MecanumDrivePoseEstimator.
        Parameters:
        gyroAngle - The current gyro angle.
        initialPoseMeters - The starting pose estimate.
        kinematics - A correctly-configured kinematics object for your drivetrain.
        stateStdDevs - Standard deviations of model states. Increase these numbers to trust your model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
        localMeasurementStdDevs - Standard deviations of the encoder and gyro measurements. Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is in the form [theta], with units in radians.
        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]ᵀ, with units in meters and radians.
      • MecanumDrivePoseEstimator

        public MecanumDrivePoseEstimator​(Rotation2d gyroAngle,
                                         Pose2d initialPoseMeters,
                                         MecanumDriveKinematics kinematics,
                                         Matrix<N3,​N1> stateStdDevs,
                                         Matrix<N1,​N1> localMeasurementStdDevs,
                                         Matrix<N3,​N1> visionMeasurementStdDevs,
                                         double nominalDtSeconds)
        Constructs a MecanumDrivePoseEstimator.
        Parameters:
        gyroAngle - The current gyro angle.
        initialPoseMeters - The starting pose estimate.
        kinematics - A correctly-configured kinematics object for your drivetrain.
        stateStdDevs - Standard deviations of model states. Increase these numbers to trust your model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
        localMeasurementStdDevs - Standard deviations of the encoder and gyro measurements. Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is in the form [theta], with units in radians.
        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]ᵀ, with units in meters and radians.
        nominalDtSeconds - The time in seconds between each robot loop.
    • Method Detail

      • setVisionMeasurementStdDevs

        public void setVisionMeasurementStdDevs​(Matrix<N3,​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]ᵀ, with units in meters and radians.
      • resetPosition

        public void resetPosition​(Pose2d poseMeters,
                                  Rotation2d gyroAngle)
        Resets the robot's position on the field.

        You NEED to reset your encoders (to zero) when calling this method.

        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:
        poseMeters - The position on the field that your robot is at.
        gyroAngle - The angle reported by the gyroscope.
      • getEstimatedPosition

        public Pose2d getEstimatedPosition()
        Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
        Returns:
        The estimated robot pose in meters.
      • addVisionMeasurement

        public void addVisionMeasurement​(Pose2d visionRobotPoseMeters,
                                         double timestampSeconds,
                                         Matrix<N3,​N1> visionMeasurementStdDevs)
        Add a vision measurement to the Unscented 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 update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds) every loop.

        Note that the vision measurement standard deviations passed into this method will continue to apply to future measurements until a subsequent call to 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 updateWithTime(double, edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds) 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 measurements. Increase these numbers to trust global measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
      • update

        public Pose2d update​(Rotation2d gyroAngle,
                             MecanumDriveWheelSpeeds wheelSpeeds)
        Updates the the Unscented Kalman Filter using only wheel encoder information. This should be called every loop, and the correct loop period must be passed into the constructor of this class.
        Parameters:
        gyroAngle - The current gyro angle.
        wheelSpeeds - The current speeds of the mecanum drive wheels.
        Returns:
        The estimated pose of the robot in meters.
      • updateWithTime

        public Pose2d updateWithTime​(double currentTimeSeconds,
                                     Rotation2d gyroAngle,
                                     MecanumDriveWheelSpeeds wheelSpeeds)
        Updates the the Unscented Kalman Filter using only wheel encoder information. This should be called every loop, and the correct loop period must be passed into the constructor of this class.
        Parameters:
        currentTimeSeconds - Time at which this method was called, in seconds.
        gyroAngle - The current gyroscope angle.
        wheelSpeeds - The current speeds of the mecanum drive wheels.
        Returns:
        The estimated pose of the robot in meters.