Advanced Pose Estimation: Multi-Tag Fusion and Standard Deviations

Basic odometry drifts; basic vision is jumpy. The art of competitive localization is fusing them with the right trust levels so the robot knows where it is to a few centimeters across a whole match. WPILib's SwerveDrivePoseEstimator does this via Kalman-style standard deviations.

Standard deviations are the trust knobs

The estimator weighs each source by its declared uncertainty. Smaller std-dev = more trust. You set odometry trust at construction and vision trust per measurement (or globally):

m_poseEstimator = new SwerveDrivePoseEstimator(
    m_kinematics, gyroAngle, modulePositions, initialPose,
    VecBuilder.fill(0.05, 0.05, 0.01),   // state (odometry) std devs: x,y,theta
    VecBuilder.fill(0.5, 0.5, 9999999)); // default vision std devs

A huge theta std-dev for vision tells the estimator to ignore vision heading and trust the gyro -- the standard MegaTag2 pattern, since the IMU is more reliable for yaw than a single tag.

Scale trust with distance and tag count

The smart move is dynamic std-devs: trust vision more when you see multiple tags up close, less when one tag is far away (where small angle errors mean large position errors).

var est = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("");
if (est != null && est.tagCount > 0) {
  double xyStd = (est.tagCount >= 2) ? 0.5 : 1.2;        // tighter with 2+ tags
  xyStd *= (1 + est.avgTagDist * est.avgTagDist * 0.1);  // looser when far
  m_poseEstimator.setVisionMeasurementStdDevs(
      VecBuilder.fill(xyStd, xyStd, 9999999));
  m_poseEstimator.addVisionMeasurement(est.pose, est.timestampSeconds);
}

tagCount, avgTagDist, pose, and timestampSeconds are all fields on the LimelightHelpers PoseEstimate struct.

Reject obviously bad data

Guard the update: skip measurements while spinning fast (gyroRate > 720 deg/s), skip poses that land off the field or absurdly far from the current estimate, and skip when tagCount == 0. One bad teleport into the estimator can ruin an auto-align.

Latency compensation

Vision data is always a little old. The estimator latency-compensates if you pass the measurement timestamp (which MegaTag2's timestampSeconds provides) -- it rewinds, inserts the vision sample, and replays odometry forward. Never pass Timer.getFPGATimestamp() as the vision time; pass the camera's capture timestamp.

The payoff

With tuned fusion you can run closed-loop drive-to-pose for auto-scoring and trust odometry through brief vision dropouts. Validate in AdvantageScope's field view: the fused pose should track smoothly and snap gently toward tags, never teleport. This is the difference between vision that helps and vision that fights you.