Class DifferentialDrivePoseEstimator

    • Constructor Detail

      • DifferentialDrivePoseEstimator

        public DifferentialDrivePoseEstimator​(Rotation2d gyroAngle,
                                              Pose2d initialPoseMeters,
                                              Matrix<N5,​N1> stateStdDevs,
                                              Matrix<N3,​N1> localMeasurementStdDevs,
                                              Matrix<N3,​N1> visionMeasurementStdDevs)
        Constructs a DifferentialDrivePoseEstimator.
        Parameters:
        gyroAngle - The current gyro angle.
        initialPoseMeters - The starting pose estimate.
        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, dist_l, dist_r]ᵀ, 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 [dist_l, dist_r, theta]ᵀ, with units in meters and 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.
      • DifferentialDrivePoseEstimator

        public DifferentialDrivePoseEstimator​(Rotation2d gyroAngle,
                                              Pose2d initialPoseMeters,
                                              Matrix<N5,​N1> stateStdDevs,
                                              Matrix<N3,​N1> localMeasurementStdDevs,
                                              Matrix<N3,​N1> visionMeasurementStdDevs,
                                              double nominalDtSeconds)
        Constructs a DifferentialDrivePoseEstimator.
        Parameters:
        gyroAngle - The current gyro angle.
        initialPoseMeters - The starting pose estimate.
        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, dist_l, dist_r]ᵀ, 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 [dist_l, dist_r, theta]ᵀ, with units in meters and 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.