Package frc.robot.subsystems.apriltagvision

Class AprilTagPoseEstimator

java.lang.Object

frc.robot.subsystems.apriltagvision.AprilTagPoseEstimator

public class AprilTagPoseEstimator
extends Object

Processes AprilTag pose observations and produces vision measurements suitable for pose estimation.

This class encapsulates the filtering and standard deviation calculation logic, making it testable without requiring PhotonVision, subsystem infrastructure, or AdvantageKit logging.

Nested Class Summary

Nested Classes

Modifier and Type	Class	Description
static final record	AprilTagPoseEstimator.EstimationResult	Result of evaluating a pose observation.
static final record	AprilTagPoseEstimator.Params	Parameters for pose estimation filtering and uncertainty calculation.
static enum	AprilTagPoseEstimator.RejectionReason	Reason a pose observation was rejected by the estimator.
static final record	AprilTagPoseEstimator.VisionMeasurement	A validated vision measurement ready for pose estimation fusion.

Constructor Summary

Constructors

Constructor	Description
AprilTagPoseEstimator(AprilTagPoseEstimator.Params params, Supplier<edu.wpi.first.math.geometry.Pose2d> odometryPoseSupplier)	Creates a new AprilTagPoseEstimator.

Method Summary

Modifier and Type	Method	Description
AprilTagPoseEstimator.EstimationResult	estimate(AprilTagVisionIO.PoseObservation observation, String cameraName)	Processes a pose observation and returns an estimation result.
boolean	isWithinInitialAcceptanceWindow()	Returns whether the estimator is still within the initial pose acceptance window.

Methods inherited from class java.lang.Object

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

Constructor Details

AprilTagPoseEstimator

public AprilTagPoseEstimator(AprilTagPoseEstimator.Params params,
 Supplier<edu.wpi.first.math.geometry.Pose2d> odometryPoseSupplier)

Creates a new AprilTagPoseEstimator.

Parameters:

params - configuration parameters for filtering and uncertainty

odometryPoseSupplier - supplier for the current odometry pose used for consistency checking

Method Details

isWithinInitialAcceptanceWindow

public boolean isWithinInitialAcceptanceWindow()

Returns whether the estimator is still within the initial pose acceptance window.

While this returns true, the odometry deviation filter is bypassed so the pose estimator can lock onto a real field position at startup.

Returns:

true if the accepted measurement count has not yet reached the configured initial acceptance count

estimate

public AprilTagPoseEstimator.EstimationResult estimate(AprilTagVisionIO.PoseObservation observation,
 String cameraName)

Processes a pose observation and returns an estimation result.

Standard deviations are scaled by distance squared divided by tag count, using meters for distance. When the set of observed tag IDs changes between consecutive frames from the same camera, the standard deviations are further multiplied by the tag-switch multiplier to dampen jitter from same-face tag pairs.

Parameters:

observation - the raw pose observation from vision

cameraName - the name of the camera that produced this observation

Returns:

estimation result containing the measurement or rejection reason