Software · Localization

Pose Estimator Lab

Drive a swerve robot around the 2026 REBUILT field. Wheel odometry slowly loses track of where the robot really is. Three cameras on an Nvidia Jetson read AprilTags, and a Kalman filter blends the two. New here? Press Walkthrough.

Driving

W A S D move the robot field-relative, as seen from the blue driver station at the bottom. Q and E turn it. On a phone, hold the keys here.

Top speedm/s 4.0

The 2026 robot tops out at 5.0 m/s (SPEED_AT_12V_MPS) and turns at up to 4.7 rad/s here.

Odometry

Odometry means working out where you are by adding up how far the wheels turned, with the gyro giving the heading.

Wheel drift% of distance 3.0

Worn treads, carpet seams and wheel scrub make each wheel's distance a little wrong. Those errors add up the farther you drive. Pushing into the hub, a trench wall or the guardrail makes it worse, because the wheels keep turning while the robot doesn't move.

Slip on the bumps% of distance 15

On a bump the wheels roll along a 0.29 rad (16°) slope, so they count about 4% more distance than the robot covers across the floor. They also skid as the robot tips over the peak.

Gyro drift% of turning 0.3

The gyro is very good, but not perfect. At 0.3%, one full turn (6.28 rad) leaves the heading about 0.019 rad off.

Cameras

Camera pose error× a typical reading ×1.0

How far each reading lands from the truth. It grows with tag distance, with how sharply the camera sees the tag, and with motion blur from driving or spinning while the picture is taken.

Alliance

Three global-shutter cameras face left, right and back. Left and right sit 0.29 m off the floor, the back one 0.54 m. All tilt up 0.17 rad (10°) and see about 1.35 rad (77°) side to side. A Jetson finds tags with a GPU detector at 60 frames a second per camera, solves the robot's pose, and sends it to the roboRIO with the time the picture was taken. The hub and trench walls block the view of anything behind them.

Localization state

RobotContainer asks for SHOOTING_FOCUS while aiming and shooting, and FULL the rest of the time. In a focus state, a reading that includes a focus tag (outlined on the field) uses the focused σ. Every other reading uses the not-focused σ. No tags are thrown away. See state machines.

Vision filter

Every reading must pass these checks, in this order, before it reaches the Kalman filter. The defaults are the 2026 robot's values from LocalizationConstants.

Always on: a reading outside the field walls is thrown away.

Ideas not in the 2026 code

Reject readings while on a bump
The robot is tilted, so the pose is off.

Kalman filter

σ (sigma) is a standard deviation: how far off you expect a value to be. Small σ means "trust this a lot".

Trust vision less while moving
Not in the 2026 code. Adds 25% to σ for every m/s of speed.

Each accepted reading pulls the estimate a fraction q ÷ (q + σ) of the way toward it. That's the WPILib pose estimator's gain. With the 2026 numbers (q 0.1, σ 0.9) each reading pulls 10% of the way, in x, y and heading. The starter σ of 5.0 in the Computer Vision lesson pulls only 2%.

Field

Blue driver station view

True robot Odometry Fused estimate Accepted, circle = σ ×Rejected Our alliance zone Focus tag

Where the robot is

True pose–

Odometry (smooth pose)–

Fused (field pose)–

Odometry error–

Fused error–

Within 0.06 m and 0.017 rad?–

Distance driven–

Speed–

Surface–

State–

The robot runs two estimators. The smooth pose only uses odometry. The field pose also takes the camera readings. The tolerance is the one the Computer Vision lesson checks.

What each camera sees

Values hold for 250 ms so you can read them

Camera	Tags	Dist	Result

Why each reading is off

Expected error (1σ) = distance error × viewing-angle factor + motion blur + bump tilt. The robot never sees this number. The 2026 code gives every reading the same σ, so a blurry far reading counts as much as a sharp close one.

Filter results

Last 2 seconds of readings

Position error vs time

Odometry onlyFused estimate

Things to try

Turn every camera off, then spin in place with Q for a few turns. Watch the heading part of the fused error grow. Turn them back on and watch it shrink.
Set Vision σ to 0.05. The estimate follows every noisy reading and jitters. Set it to 5.0 and it barely moves. Where is the sweet spot?
Drive into our alliance zone and watch for Too few tags in our zone. Why might single-tag readings be risky near the wall?
Get shoved, then drive to a hub. How long until the fused estimate is back within 0.06 m?

Read more

AprilTag Fiducials: how tags are found and turned into a pose
Nvidia Jetson Vision Computer
Objects and Odometry: build a pose estimator
Computer Vision: add camera readings to it