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.