SwerveDriveOdometry vs. SwerveDrivePoseEstimator

If you've already built your drivetrain subsystem with SwerveDriveOdometry (introduced in Unit 7), the first change this lesson requires is upgrading to SwerveDrivePoseEstimator. They serve the same basic function — tracking the robot's field-relative pose using encoder and gyro data — but the pose estimator adds the ability to receive external measurements (vision) and fuse them into the tracked pose.

The Kalman Filter: What It Does and Why It Works

You don't need to understand the math of a Kalman filter to use SwerveDrivePoseEstimator correctly. But you do need to understand the conceptual model, because it explains every API decision and every tuning choice you'll make in Lessons 7 and 8.

At its core, the Kalman filter maintains two things simultaneously: a best estimate of the robot's current state (pose), and an estimate of the uncertainty in that estimate. Every 20 ms when update() is called, the filter integrates the encoder and gyro measurements — which adds small amounts of uncertainty (wheel slip, encoder error). When addVisionMeasurement() is called, the filter incorporates the vision measurement by blending it with the current estimate, weighted by how trustworthy each source is relative to the other.

The weights are determined by the standard deviations you provide:

• Odometry standard deviations (set in the constructor): how much the Kalman filter trusts the encoder/gyro integration per loop. Smaller = trust odometry more. Usually small values like [0.1, 0.1, 0.1] meters/radians.
• Vision standard deviations (passed with each measurement): how much the Kalman filter trusts this specific vision measurement. Larger = trust vision less for this measurement. Variable — should reflect how accurate the current detection actually is (distance, number of tags, etc.).

Fusion in Action: Odometry Drift vs. Vision-Corrected Pose

The visualizer below simulates a robot moving along a curved path for several seconds. Watch how odometry alone drifts from the true position, and how adding periodic vision measurements snaps the estimate back. Adjust drift rate and vision update frequency to understand the relationship.

Implementing the Fusion in DriveSubsystem

The architecture is clear: SwerveDrivePoseEstimator lives in the drivetrain subsystem, not in the vision subsystem. Vision measurements are passed into the drivetrain via a public method. This keeps the drivetrain as the single source of truth for robot pose, and the vision subsystem as a data provider — never a pose owner.

import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.kinematics.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.*;

public class DriveSubsystem extends SubsystemBase {

    // ... motor controllers, modules, gyro declared here ...

    private final SwerveDrivePoseEstimator m_poseEstimator;
    private final Field2d m_field = new Field2d();

    public DriveSubsystem() {
        // Odometry standard deviations: [x (m), y (m), theta (rad)]
        // These represent how much error accumulates per loop from encoders/gyro.
        // Lower = trust odometry more for the blend calculation.
        // Typical starting values: 0.1 m, 0.1 m, 0.1 rad
        Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);

        // Vision standard deviations: default for addVisionMeasurement() calls
        // that don't specify their own. We'll override these per-measurement
        // in the actual addVisionMeasurement() call based on detection quality.
        Matrix<N3, N1> visionStdDevs = VecBuilder.fill(0.9, 0.9, 0.9);

        m_poseEstimator = new SwerveDrivePoseEstimator(
            m_kinematics,
            m_gyro.getRotation2d(),
            getModulePositions(),
            new Pose2d(),         // reset to actual starting pose in autonomousInit()
            stateStdDevs,
            visionStdDevs
        );

        SmartDashboard.putData("Field", m_field);
    }

    @Override
    public void periodic() {
        // Update with encoder + gyro every loop — this is identical to
        // SwerveDriveOdometry.update(), same signature.
        m_poseEstimator.update(
            m_gyro.getRotation2d(),
            getModulePositions()
        );

        // Update Field2d widget for SmartDashboard visualization
        m_field.setRobotPose(getPose());
        SmartDashboard.putNumber("Drive/X", getPose().getX());
        SmartDashboard.putNumber("Drive/Y", getPose().getY());
    }

    // ── Public methods ────────────────────────────────────────────────────────

    /** Called by VisionSubsystem whenever a new pose estimate is ready. */
    public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds) {
        m_poseEstimator.addVisionMeasurement(visionPose, timestampSeconds);
    }

    /** Overload: addVisionMeasurement with explicit per-measurement stddevs.
     *  Use this when measurement quality varies (multi-tag vs single-tag,
     *  close vs far). Lesson 7 covers how to compute these dynamically. */
    public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds,
                                     Matrix<N3, N1> stdDevs) {
        m_poseEstimator.addVisionMeasurement(visionPose, timestampSeconds, stdDevs);
    }

    public Pose2d getPose() {
        return m_poseEstimator.getEstimatedPosition();
    }

    public void resetPose(Pose2d pose) {
        m_poseEstimator.resetPosition(
            m_gyro.getRotation2d(),
            getModulePositions(),
            pose
        );
    }

    private SwerveModulePosition[] getModulePositions() {
        return new SwerveModulePosition[] {
            m_frontLeft.getPosition(),
            m_frontRight.getPosition(),
            m_backLeft.getPosition(),
            m_backRight.getPosition()
        };
    }
}

The Architecture: Who Calls What

The flow of data from camera to robot pose has exactly one "write" point into the pose estimator, and the drivetrain owns it. The vision subsystem is a producer of measurements, not a consumer of poses.

The Vision Subsystem Side

For completeness, here is the full VisionSubsystem that integrates with the drivetrain architecture above. This uses PhotonVision (Lessons 3–4) but the pattern is identical for Limelight (Lesson 5) — only the measurement source changes.

public class VisionSubsystem extends SubsystemBase {

    private final DriveSubsystem m_drive;
    private final PhotonCamera m_camera = new PhotonCamera("FrontCamera");
    private final PhotonPoseEstimator m_poseEstimator;

    public VisionSubsystem(DriveSubsystem drive) {
        m_drive = drive;
        AprilTagFieldLayout layout =
            AprilTagFields.k2025ReefScapeV2.loadAprilTagLayoutField();
        m_poseEstimator = new PhotonPoseEstimator(layout,
            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
            m_camera, VisionConstants.ROBOT_TO_FRONT_CAMERA);
        m_poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
    }

    @Override
    public void periodic() {
        Optional<EstimatedRobotPose> estimateOpt = m_poseEstimator.update();

        estimateOpt.ifPresent(estimate -> {
            Pose2d visionPose     = estimate.estimatedPose.toPose2d();
            double captureTime  = estimate.timestampSeconds;
            int    numTagsUsed  = estimate.targetsUsed.size();

            // Basic quality check before passing to drivetrain.
            // Lesson 8 covers more sophisticated filtering.
            // For now: reject if pose is outside the field boundaries.
            if (visionPose.getX() > 0 && visionPose.getX() < 16.54 &&
                visionPose.getY() > 0 && visionPose.getY() < 8.21) {

                // Pass to drivetrain with the capture timestamp — not Timer.getFPGATimestamp()
                m_drive.addVisionMeasurement(visionPose, captureTime);

                SmartDashboard.putNumber("Vision/TagsUsed", numTagsUsed);
                SmartDashboard.putNumber("Vision/VisionX", visionPose.getX());
                SmartDashboard.putNumber("Vision/VisionY", visionPose.getY());
            }
        });
    }
}

Verifying Fusion Is Working

Vision fusion can be silently broken — the code compiles, runs, and even produces reasonable pose estimates — but the fusion itself may not be contributing anything. These are the tests that distinguish "working" from "looks like working."

Test 1: The vision measurement counter

Add a counter in your drivetrain's addVisionMeasurement() method that increments each time it's called. Log it to SmartDashboard. During autonomous when the robot faces tags, it should increment at roughly the camera's frame rate (30–60 times per second). If it's 0 or increments only once, the vision subsystem isn't calling through correctly.

Test 2: The drift correction test

Disable vision temporarily (comment out the addVisionMeasurement() call). Run a 3-meter path and mark the landing position with tape. Re-enable vision. Run the same path. The landing position with vision correction should be more consistent and closer to the target. If there's no visible difference, check that fusion is actually being called (Test 1) and that the standard deviations aren't set so high that vision measurements are ignored.

Test 3: The Field2d overlay

Open SmartDashboard's Field widget and add a second robot object for the raw vision pose (before fusion): m_field.getObject("visionPose").setPose(visionPose). You should see two robot icons — the green fused estimate and the orange raw vision estimate. They should generally agree within 5–10 cm when the camera is facing tags clearly. If the vision icon is jumping wildly while the fused estimate stays smooth, the Kalman filter is correctly dampening noisy measurements. If the fused estimate is also jumping, vision standard deviations are set too low.

Three Failure Modes That Prevent Fusion from Helping

Failure 1: Wrong timestamp — fusion happens at the wrong time

Passing Timer.getFPGATimestamp() instead of estimate.timestampSeconds means the Kalman filter applies the correction at the robot's current position, not where it was when the image was taken. The fusion still runs but applies the correction to the wrong historical state, reducing its accuracy. Symptom: fused pose oscillates slightly at the frame rate even when the robot is stationary.

Failure 2: Vision standard deviations too large — measurements ignored

If vision standard deviations are set very high (e.g., 100, 100, 100), the Kalman filter weights vision at nearly zero. addVisionMeasurement() runs without error, the counter increments, but the fused pose barely moves toward the vision estimate. Symptom: the fused estimate and odometry-only estimate are identical even when vision is producing good measurements.

Failure 3: Odometry standard deviations too low — odometry overrides everything

If odometry standard deviations are set near zero (e.g., 0.001, 0.001, 0.001), the Kalman filter trusts odometry so completely that vision corrections are ignored even when they're highly accurate. This is the inverse of Failure 2. Symptom: same as Failure 2, but identified by checking whether lowering odometry stddevs from their initial values worsened things.

🔌 System Check

Knowledge Check