Intro to Programming for FRC Teams
Unit 7: Drivetrains & Swerve Drive

Pose Estimation with SwerveDrivePoseEstimator

Complete & Continue Next Lesson Learn More
Unit 7 · Lesson 10

Pose Estimation with SwerveDrivePoseEstimator

Pure odometry drifts. It starts accurate and compounds error with every wheel slip, every gyro wobble, every defensive collision. SwerveDrivePoseEstimator — a drop-in replacement for SwerveDriveOdometry — adds a Kalman filter that fuses odometry with periodic vision measurements to correct that drift in real time. This is how championship-level autonomous works.

By the end of this lesson, you will:

  • Explain why pure odometry accumulates error and why that matters specifically during long autonomous routines
  • Replace SwerveDriveOdometry with SwerveDrivePoseEstimator in the drivetrain subsystem
  • Call addVisionMeasurement(pose, timestamp) to inject a vision-based pose correction
  • Configure state standard deviations and vision standard deviations to tune how much the filter trusts each source
  • Filter bad vision measurements using ambiguity and distance thresholds before injecting them
  • Distinguish between correcting with addVisionMeasurement() vs. resetting with resetPose() and when to use each

Why Odometry Alone Isn't Enough

Lesson 4's odometry is accurate for short distances and benign conditions. Over a full 15-second autonomous with defensive contact, multiple fast turns, and carpet variations, three error sources compound:

  • Wheel slip — a single defense contact can add 5–10 cm of positional error instantly
  • Encoder resolution — integer tick quantization in TalonFX at high gear ratios accumulates into millimeter-scale noise every loop
  • Gyro drift — the Pigeon 2 is excellent but still drifts 0.5–2° over a full match under vibration

For scoring games that require 2–3 cm accuracy at the target, 10–15 cm of total odometry drift after two cycles means missed shots, failed game piece pickups, and wasted autonomous points. Vision-based correction is not optional at the championship level — it's the baseline.

Drift Accumulation — Visualized

Select a scenario to see how odometry drift accumulates and how vision measurements correct it.

Pose estimate over a 15-second autonomous
0m 5m 10m
True path (ground truth)
Odometry estimate
Pose estimator (with vision)
← select a scenario to see how vision corrections affect the pose estimate

The Kalman Filter: A Weighted Average Between Two Sources

The Kalman filter is the math inside SwerveDrivePoseEstimator. You don't need to implement it — WPILib does that. But you need to understand its input parameters, because they control how it works.

The filter maintains a pose estimate that is a weighted average between two sources:

  • Prediction (odometry) — continuous 50 Hz updates from wheel encoders and gyro. High frequency, gradual drift.
  • Measurement (vision) — periodic updates from camera/AprilTag detection. Low frequency (5–30 Hz), high accuracy when the tag is clearly visible.

The weights are controlled by standard deviations. A small standard deviation means "I trust this source a lot." A large standard deviation means "I'm uncertain about this." The filter blends the two sources, giving more weight to whichever has smaller standard deviation. When vision sees a tag clearly up close, its standard deviation is small and the filter trusts it heavily. When the tag is far away or ambiguous, the standard deviation is large and the filter relies more on odometry.

💡 Think of it like GPS + dead reckoning in your car's nav system

Your phone's navigation uses GPS position (periodic, potentially noisy) and accelerometer dead reckoning (continuous, drifts). When GPS has a good signal, the nav trusts GPS heavily and corrects the accumulated accelerometer drift. In a tunnel with no GPS, it trusts the accelerometer only. The Kalman filter does exactly this — automatically weighting each source based on configured uncertainty parameters.

The Upgrade: From Odometry to PoseEstimator

SwerveDrivePoseEstimator is a drop-in replacement for SwerveDriveOdometry. The method signatures for update(), resetPosition(), and getPoseMeters() are identical. The constructor takes two additional arguments: state standard deviations and vision standard deviations.

SwerveDrivetrain.java — upgrading from Odometry to PoseEstimator
// ── Field declaration ─────────────────────────────────
−private final SwerveDriveOdometry m_odometry;
+private final SwerveDrivePoseEstimator m_poseEstimator;

// ── Constructor ───────────────────────────────────────
−m_odometry = new SwerveDriveOdometry(m_kinematics, getHeading(), getModulePositions(), new Pose2d());
+m_poseEstimator = new SwerveDrivePoseEstimator(
+  m_kinematics, getHeading(), getModulePositions(), new Pose2d(),
+  VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), // state std devs: x, y, theta
+  VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)) // vision std devs: x, y, theta
+);

// ── periodic() — identical call, different object name ─
−m_odometry.update(getHeading(), getModulePositions());
+m_poseEstimator.update(getHeading(), getModulePositions());

// ── getPose() — identical getter, different object ─────
−return m_odometry.getPoseMeters();
+return m_poseEstimator.getEstimatedPosition();

Adding Vision Measurements

The new method that SwerveDrivePoseEstimator provides — not available on SwerveDriveOdometry — is addVisionMeasurement(). This is the injection point for every camera-based pose estimate. Call it whenever your vision system detects an AprilTag and produces a reliable pose estimate.

SwerveDrivetrain.java — addVisionMeasurement and filtering
/** Called by a vision subsystem whenever a reliable pose is available. */
public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds) {
  m_poseEstimator.addVisionMeasurement(visionPose, timestampSeconds);
}

/** Overload with per-measurement std devs for adaptive trust. */
public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds,
                                      Matrix<N3,N1> stdDevs) {
  m_poseEstimator.addVisionMeasurement(visionPose, timestampSeconds, stdDevs);
}

// ── Typical call site: inside a vision command or periodic ──
// Get pose estimate from vision library (PhotonVision example)
Optional<EstimatedRobotPose> result = m_photonEstimator.update();
result.ifPresent(est -> {

  // Filter bad measurements before injecting
  double distance = getDistanceToTag(est);
  double ambiguity = getBestTargetAmbiguity(est);

  if (ambiguity > 0.9 || distance > 5.0) {
    return; // reject — too unreliable
  }

  // Scale std devs based on distance and tag count
  Matrix<N3,N1> adaptiveStdDevs = computeStdDevs(est, distance);

  // Inject: use camera capture timestamp, not current time
  m_drive.addVisionMeasurement(
    est.estimatedPose.toPose2d(),
    est.timestampSeconds,
    adaptiveStdDevs);
});

// ── Adaptive std devs helper ─────────────────────────────
private Matrix<N3,N1> computeStdDevs(EstimatedRobotPose est, double distM) {
  int numTags = est.targetsUsed.size();
  double xyStd, thetaStd;

  if (numTags >= 2) {
    // Multi-tag: very reliable — trust proportional to distance²
    xyStd = 0.5 * distM * distM;
    thetaStd = 0.5 * distM * distM;
  } else {
    // Single-tag: less reliable — higher base uncertainty
    xyStd = 1.0 * distM * distM;
    thetaStd = Double.MAX_VALUE; // don't correct heading from 1 tag
  }
  return VecBuilder.fill(xyStd, xyStd, thetaStd);
}
← click a highlighted token

Standard Deviation Tuning

The standard deviations control everything. Set them wrong and the filter either ignores vision (too high) or twitches on every noisy measurement (too low). Use this tool to find the right starting values for your scenario.

Standard deviation advisor — select your vision scenario
What is your measurement situation?
← select a scenario for recommended standard deviations
⚠️ Don't call resetPose() on every vision measurement

A common mistake: the vision pipeline fires and the programmer calls resetPose(visionPose). This is wrong. resetPose() discards all accumulated odometry and starts fresh — if called 10 times per second, odometry never accumulates. The robot's position jumps discontinuously with every camera frame, and the field-oriented control's heading reference becomes unstable. addVisionMeasurement() is the correct call — it blends the vision data with existing odometry through the filter rather than replacing it. Reserve resetPose() for intentional position resets: the start of autonomous, or a known-good physical reference position.

The Timestamp: Capture Time, Not Processing Time

The second argument to addVisionMeasurement() is the timestamp of when the image was captured, not when your code processed it. There is a real delay (50–200 ms in practice) between when the camera captures a frame, the co-processor detects the AprilTag, the result is transmitted over NetworkTables, and your robot code receives it.

If you pass the current robot time as the timestamp, the Kalman filter applies the vision correction to the robot's current pose — but the vision measurement was taken 100 ms ago when the robot was somewhere different. The filter then jumps the estimate to account for the discrepancy, which appears as a position glitch. Using the actual capture timestamp lets the filter correctly retroactively correct the pose estimate at the right point in time.

PhotonVision and Limelight both provide the capture timestamp in their result objects. Always use it.

Vision Library Integration Sketches

Full vision pipeline setup is Unit 10's territory. But here is the call-site pattern for both major libraries so you know where the addVisionMeasurement() call lives in each integration.

Vision integration patterns — PhotonVision and Limelight
// ── PhotonVision — PhotonPoseEstimator ───────────────
private final PhotonPoseEstimator m_photonEstimator =
  new PhotonPoseEstimator(
    kFieldLayout,                // AprilTag field layout
    PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
    m_camera,                  // PhotonCamera object
    kRobotToCamera             // Transform3d camera offset
  );

// In periodic() or a vision command's execute():
m_photonEstimator.update().ifPresent(est -> {
  if (isMeasurementValid(est)) {
    m_drive.addVisionMeasurement(
      est.estimatedPose.toPose2d(),
      est.timestampSeconds,
      computeStdDevs(est));
  }
});

// ── Limelight — NetworkTables JSON ───────────────────
LimelightHelpers.SetRobotOrientation(
  "limelight", getHeading().getDegrees(), 0, 0, 0, 0, 0);

LimelightHelpers.PoseEstimate mt2 =
  LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");

if (mt2 != null && mt2.tagCount > 0) {
  m_drive.addVisionMeasurement(
    mt2.pose,
    mt2.timestampSeconds);
}
← click a highlighted token
🔍 LRI Perspective: "Vision fusion separates Regionals-level from Worlds-level autonomous"

At a Regional with well-maintained fields, pure odometry is often adequate — the field is flat, the alliance station is predictable, and the robot hasn't been hit hard enough to slip significantly. At a Championship event, the fields see hundreds of matches. Carpet compresses. Tags get bumped. Robots take harder hits. I've watched teams who were dominant at Regionals struggle at Worlds specifically because their autonomous relied on pure odometry that didn't survive a week of Championship-level play. The teams with vision fusion just kept scoring. It's not magic — it's correcting for physical reality. Build it in from the start.

🔌 System Check — Pose Estimator Verification

Before relying on pose estimation for autonomous:

  • Verify the upgrade didn't break anything: After replacing SwerveDriveOdometry with SwerveDrivePoseEstimator, drive the square test from Lesson 4 and confirm the Field2d widget still shows accurate position tracking. The update() call is identical — if odometry was working, it should still work.
  • Verify vision measurement is being called: Add SmartDashboard.putNumber("Vision/MeasurementsAdded", m_visionMeasCount++) to your addVisionMeasurement() method. Watch this counter in Shuffleboard while vision is running — it should increment every time the camera sees a tag. If it never increments, vision measurements are not reaching the estimator.
  • Compare filtered vs raw estimates: Publish both the pose estimator output and the raw odometry position (before vision fusion) to SmartDashboard. In AdvantageScope, overlay both on the field. When the camera sees a tag, the filtered estimate should visibly converge toward the tag-based pose while the raw odometry continues to drift.
  • Verify timestamps: Check that the difference between the vision timestamp and the current FPGA timestamp is a reasonable camera latency (30–200 ms). If the difference is near zero, you're using current time instead of capture time — a guaranteed source of ghost jumps in the pose estimate.

Knowledge Check

1. Your vision pipeline runs at 20 Hz (every 50 ms) and the camera has 80 ms of latency. You call addVisionMeasurement(visionPose, Timer.getFPGATimestamp()) using the current time. What error does this introduce?

  • A No error — the Kalman filter automatically compensates for any timestamp offset.
  • B The filter applies the vision correction to the robot's pose at the current time, but the vision data was captured 80 ms ago when the robot was somewhere different. The filter compensates for 80 ms of additional movement that didn't happen, causing the pose estimate to jump. Use the camera's capture timestamp, not the current time.
  • C The filter throws an exception if the timestamp is in the future relative to the capture time.
  • D The 80 ms difference causes the vision measurement to be silently rejected.

2. A vision measurement has an ambiguity of 0.95. Your filter code does not check ambiguity before calling addVisionMeasurement(). What happens to the pose estimate?

  • A The measurement is handled normally — ambiguity only affects the visual overlay in the dashboard.
  • B A high-ambiguity pose estimate from a single AprilTag may be completely wrong — the tag's two possible interpretations are nearly equally likely. Injecting it can corrupt the pose estimate with a wildly incorrect position, potentially causing the robot to drive to the wrong location in autonomous. Reject measurements above ~0.9 ambiguity.
  • C The pose estimator detects high ambiguity and automatically applies a larger standard deviation.
  • D High ambiguity only affects single-axis measurements — the XY position remains reliable.

3. The vision standard deviations for a single-tag measurement at 4 meters are set to VecBuilder.fill(0.05, 0.05, 0.05) — very small, indicating high trust. What is the risk?

  • A The Kalman filter will ignore odometry entirely and rely only on vision.
  • B Single-tag pose estimates at 4 meters have significant 3D projection error — the physical accuracy is much worse than 5 cm. Telling the filter to trust them at 5 cm accuracy means noisy measurements will jerk the pose estimate discontinuously. The filter will jump repeatedly as each measurement arrives, degrading the estimate worse than odometry alone. Distance-proportional or multi-tag-dependent standard deviations are required at range.
  • C Very small standard deviations cause the filter to run out of numerical precision.
  • D Small standard deviations for vision automatically increase the state standard deviations to compensate.
💪 Practice Prompt

Upgrade to SwerveDrivePoseEstimator

  1. In SwerveDrivetrain.java, replace the SwerveDriveOdometry field and constructor call with SwerveDrivePoseEstimator. Use starting standard deviations of VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)) for state and VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)) for vision. Update getPose() to call getEstimatedPosition() instead of getPoseMeters(). Run the square test — confirm Field2d still tracks correctly.
  2. Add the addVisionMeasurement(Pose2d, double) and addVisionMeasurement(Pose2d, double, Matrix<N3,N1>) overloads to the drivetrain's public API. Add a counter field m_visionMeasCount that increments on each call. Publish it to SmartDashboard. Manually call addVisionMeasurement(new Pose2d(1.0, 1.0, new Rotation2d()), Timer.getFPGATimestamp()) from a test trigger and confirm the counter increments and the Field2d widget shows the robot snap to (1.0, 1.0).
  3. Add the computeStdDevs() helper and the ambiguity/distance filtering logic shown in the code explorer. Stub out the vision measurement source: create a periodic method that — for testing purposes only — calls addVisionMeasurement() with a fixed "correct" pose every 500 ms. Watch Field2d during driving — confirm the estimate periodically corrects toward the stubbed pose.
  4. Stretch goal: If your team has a PhotonVision or Limelight camera, install the respective library (PhotonLib or LimelightHelpers) and complete the integration sketch from this lesson. Watch both the raw vision pose and the filtered estimator output in AdvantageScope as you move the robot in front of a tag. The filtered output should be smoother and more stable than the raw vision data. If the filtered output jumps more than the raw vision, your standard deviations for vision are too small — increase them until the output is stable.