Fusing Vision with Odometry

Odometry is smooth and fast but drifts. Vision is absolute and drift-free but noisy and occasional (and only when a tag is in view). Fusing them gives the best of both: a pose that updates every loop, stays smooth, and gets pulled back to the truth whenever a tag is seen.

The WPILib pose estimator

WPILib provides DifferentialDrivePoseEstimator, SwerveDrivePoseEstimator, and MecanumDrivePoseEstimator. They work like odometry but accept vision corrections. Internally they use a Kalman-filter-style approach to blend the two sources by trust.

Two method calls do the work:

• update(gyroAngle, wheelMeasurements) every loop — same as odometry, keeping the estimate current and smooth.
• addVisionMeasurement(visionPose, timestampSeconds) whenever you have a fresh vision pose. The estimator latency-compensates using the timestamp, applying the correction at the moment the image was actually captured.

// every loop
poseEstimator.update(gyro.getRotation2d(), modulePositions);

// when vision has a result
if (mt2 != null && mt2.tagCount > 0) {
  poseEstimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds);
}

Standard deviations: tuning trust

The crucial tuning knob is standard deviations — how much to trust each source. Smaller standard deviation = more trust. You set the vision trust with setVisionMeasurementStdDevs(VecBuilder.fill(xStdDev, yStdDev, thetaStdDev)) (or pass std devs per measurement in an overload of addVisionMeasurement).

• Trust vision more (smaller std devs) when many/close tags are visible.
• Trust vision less (larger std devs) at long range or with one tag.
• With MegaTag2, teams commonly distrust the vision heading entirely (a huge theta std dev, e.g. VecBuilder.fill(0.7, 0.7, 9999999)) because the gyro heading is already feeding MegaTag2 — letting vision correct it would be circular.

Filtering bad measurements

Before calling addVisionMeasurement, reject junk: no tags, the pose is off the field, the robot is rotating too fast, or the pose jumps implausibly far from the current estimate. A single bad vision frame fed in unfiltered can teleport your robot's idea of where it is and ruin an auto routine.

The payoff

With good fusion, the robot can run vision-assisted autos, line up to score from anywhere on the field, and recover from a bump or wheel slip — all while degrading gracefully to plain odometry whenever no tag is visible. This is the capstone where every sensor in this branch comes together.