FRC Odometry and Pose Estimation: Field-Centric Control with WPILib

Every good autonomous routine, every "drive to the tag and score" button, every path your robot follows without a driver depends on one deceptively simple question: where am I on the field right now? A robot has no innate sense of place. It only knows how far its wheels have turned and which way a chip on a circuit board thinks it is pointing. Turning those raw sensor readings into a trustworthy (x, y, heading) position — a pose — is the job of odometry and pose estimation.

This guide walks through how a WPILib robot tracks its own location: the difference between dead-reckoning odometry and full pose estimation, how the Kinematics, Odometry, and SwerveDrivePoseEstimator classes fit together, how a gyro like the NavX2 or Pigeon 2 supplies heading, how AprilTag vision measurements get fused in, and why coordinate frames trip up so many teams. Everything here uses stable WPILib APIs and physics that do not change with the season.

What "pose" actually means

In WPILib, your robot's location is a Pose2d — a Translation2d (an x and y position, in meters) plus a Rotation2d (a heading angle). That's three numbers describing where the robot is and which way it faces on a flat field.

The coordinate frame those numbers live in matters enormously. Per the WPILib coordinate system docs, WPILib uses the NWU convention: the positive X axis points ahead (away from your alliance wall, down the field), the positive Y axis points to the left, and Z points up. Rotation is counter-clockwise-positive when viewed from above: 0° faces along +X, 90° faces along +Y (to the left), and −90° faces right. The field origin is conventionally placed in a corner of the field so that every point on the field has positive x and y.

Two frames come up constantly:

• Field frame (also called global or world frame): fixed to the field. "The robot is at x = 4.2 m, y = 1.5 m, facing 30°." This is what odometry and pose estimation produce.
• Robot frame (robot-relative): attached to the robot. "The target is 2 m ahead and 0.5 m to my left." A camera or ultrasonic sensor reports things in this frame.

Half of all odometry bugs are really frame bugs — mixing meters and inches, feeding degrees where radians are expected, or measuring an angle clockwise when WPILib wants it counter-clockwise. Nail down your units and your sign conventions before you write a line of tracking code.

Dead reckoning: odometry from wheels and a gyro

Odometry is the process of estimating position by accumulating small movements over time. It is a form of dead reckoning — the same technique sailors used before GPS: start from a known point, measure how fast and in what direction you moved each instant, and integrate.

The building block is kinematics, the math that relates individual wheel motions to whole-robot motion. WPILib splits this cleanly:

• Inverse kinematics turns a desired robot velocity (a ChassisSpeeds object: forward velocity vx, sideways velocity vy, and rotational velocity ω) into individual wheel commands. This is what you use when driving.
• Forward kinematics turns measured wheel motions back into robot motion. This is what odometry uses when tracking.

For a swerve drive, SwerveDriveKinematics.toChassisSpeeds() performs forward kinematics from the module states, and toSwerveModuleStates() performs inverse kinematics to command them, per the WPILib swerve kinematics docs. WPILib provides SwerveDriveOdometry, DifferentialDriveOdometry, and MecanumDriveOdometry classes that wrap this up. Each odometry loop, you hand the class two things: the current gyro heading and the current wheel positions (distance traveled and, for swerve, module angle), and it returns an updated Pose2d.

Where the numbers come from

The wheel distances come from encoders — typically the integrated encoders inside a NEO, Kraken, or Falcon, or an external encoder on the wheel shaft. To convert encoder counts into meters of travel you need the drive gear ratio and the wheel circumference:

distance_meters = (motor_rotations / gear_ratio) * (π * wheel_diameter_meters)

Getting this conversion exactly right is the single most important calibration in odometry. If your gear ratio is off by 2%, every distance you report is off by 2%, and the error compounds the farther you drive. It's worth measuring empirically: command the robot to drive a known distance (say 3 meters against a tape measure), read the reported odometry distance, and correct the ratio until they match. The same idea applies to a real swerve wheel whose effective diameter shrinks as the tread wears — a fresh wheel and a scrubbed-down wheel can differ by a few percent. If you want a refresher on how those ratios are built, see our guide to FRC gear ratios, and for the mechanics of the drivetrains themselves, drivetrain types and swerve drive explained.

Why heading should come from a gyro, not the wheels

You could, in principle, compute heading purely from wheel motion — a differential drive turns when its left and right sides travel different distances. Don't. Wheel-derived heading is extremely sensitive to wheel slip, scrub, and diameter error, and those errors integrate into a heading that drifts badly within seconds of aggressive driving.

Instead, feed odometry a heading straight from a dedicated gyroscope / IMU. This is the number-one rule of good odometry: trust the gyro for angle, trust the encoders for distance. WPILib's odometry classes are built around this — they take the gyro's Rotation2d as a direct input rather than deriving heading from the wheels.

Odometry's fatal flaw: drift

Dead reckoning has an unavoidable weakness. Because it works by accumulating small measurements, it also accumulates errors. Every wheel slip, every encoder quantization, every tiny gyro imperfection adds a little error that never gets corrected. Over a full match, a swerve robot's pure-odometry estimate can wander tens of centimeters — sometimes much more after a hard defensive hit that skids all four wheels.

Two sources of drift dominate:

1. Translational drift from wheel slip and diameter/gear-ratio error. Slip is worst during hard acceleration, sharp direction changes, and collisions.
2. Rotational (heading) drift from the gyro. Even good IMUs drift slowly, and heading error is especially nasty because a small angle error smears into a growing position error as you drive away.

How much does a gyro actually drift?

Modern FRC IMUs are impressively stable, which is why gyro-for-heading works so well. Two of the most common:

Those figures are from the Pigeon 2.0 product page / User's Guide and the navX2-MXP yaw-drift documentation. Two practical takeaways. First, in-motion drift is roughly two orders of magnitude worse than stationary drift — the honest numbers to plan around are the per-minute in-motion figures, not the marketing "per hour" stationary numbers. Over a 2.5-minute match that's on the order of a degree, small but real. Second, let the gyro sit still during its boot/calibration and don't touch the robot — the Pigeon's ~1°/minute "boot-into-motion" drift is the penalty for moving the robot while it initializes. For more on choosing and wiring IMUs and encoders, see the FRC sensors guide.

The point of pose estimation is to stop this drift from ever accumulating past a small bound — by periodically correcting it against a fixed reference.

Pose estimation: fusing odometry with vision

Pose estimation keeps everything odometry does but adds a second, independent source of truth: absolute measurements of where the robot is, taken from vision. When those two sources are combined intelligently, you get an estimate that has the smoothness and high update rate of odometry and the drift-free absolute accuracy of vision.

WPILib provides SwerveDrivePoseEstimator, DifferentialDrivePoseEstimator, and MecanumDrivePoseEstimator for this. Under the hood each runs a Kalman filter, which is just a principled way of blending two noisy estimates in proportion to how much you trust each one. You never write the filter math — you feed it the same odometry inputs you were already computing, plus vision measurements when they arrive, and it hands back a fused Pose2d.

The absolute reference: AprilTags

The fixed references almost every team uses today are AprilTags — flat, printed fiducial markers mounted at surveyed locations around the field. A camera running PhotonVision or a Limelight detects a tag, measures its apparent size and skew, and — because the tag's real-world position is published in an official field map (AprilTagFieldLayout) — solves backward for where the camera (and therefore the robot) must be. The result is a full field-frame pose that does not drift, because it's anchored to a physically fixed marker. For how the detection and pose-solving works, see AprilTags explained.

The catch is that a vision pose is noisy and laggy. A single frame can be off by several centimeters, the noise grows with distance from the tag and with viewing angle, and the measurement is a snapshot from tens of milliseconds ago by the time your code sees it. Odometry, by contrast, is smooth and instantaneous but drifts. The pose estimator is designed precisely to marry the two.

Using SwerveDrivePoseEstimator

The API mirrors ordinary odometry with a couple of additions. Per the WPILib API reference, the constructor takes your SwerveDriveKinematics, the current gyro Rotation2d, the array of SwerveModulePosition objects, an initial Pose2d, and optionally your state and vision standard-deviation matrices.

Then in your periodic loop:

1. Every loop (~50 Hz), call update(gyroAngle, modulePositions). This runs the odometry step exactly as before and keeps the estimate current and smooth. updateWithTime(timestamp, gyroAngle, wheelPositions) is available when you want to supply the timestamp explicitly.
2. Whenever a vision result arrives, call addVisionMeasurement(visionPose, timestampSeconds). Critically, you pass the timestamp of when the image was captured, not the current time. The estimator is latency-compensated: it rewinds its history to that moment, blends in the vision pose, and replays the odometry forward again. This is why passing an accurate capture timestamp (which PhotonVision and Limelight both provide) matters so much.
3. Read the fused result any time with getEstimatedPosition().

resetPosition() lets you re-seed the estimate — for example, snapping to a known starting pose at the beginning of autonomous.

Tuning trust with standard deviations

The single most important tuning knob is the pair of standard deviation matrices, each in the form [x, y, θ]ᵀ with units of meters and radians. They tell the Kalman filter how much to trust each source: a smaller standard deviation means "trust this more."

The defaults baked into SwerveDrivePoseEstimator (see the WPILib API reference) are:

• State (odometry) std devs: 0.1 m in x, 0.1 m in y, 0.1 rad in heading.
• Vision std devs: 0.9 m in x, 0.9 m in y, 0.9 rad in heading.

Those defaults deliberately trust odometry more than any single vision frame, so vision nudges the estimate rather than yanking it. The WPILib pose-estimator docs give three concrete rules of thumb for AprilTag fusion:

• Make the vision heading standard deviation very large. Your gyro is far more trustworthy for angle than a vision solve, so you effectively want vision to correct x and y while leaving heading to the gyro. Set the gyro/state heading std dev small and the vision θ std dev large.
• Scale the vision x/y standard deviation by distance from the tag. A tag 1 m away gives a crisp pose; the same tag 5 m away is far noisier. Increase the vision std dev with distance (many teams scale it roughly with distance, or distance squared) so far-away detections count for less.
• Reject junk before it reaches the filter. Throw out measurements that place the robot off the field, that jump implausibly far from the current estimate, or that come from a lone distant tag. Multi-tag solves are dramatically more accurate than single-tag ones — prefer them when available.

You can pass per-measurement std devs directly into addVisionMeasurement, which is exactly how you implement distance scaling: compute the std devs on the fly for each detection based on how far and how many tags it saw. setVisionMeasurementStdDevs() sets a default used by the two-argument overload.

Field-centric driving: pose estimation's payoff

The most immediately visible use of a good heading estimate is field-centric (field-oriented) driving. In robot-centric control, pushing the joystick forward always drives the robot's front forward — so if the robot is facing you, "forward" on the stick moves it toward you, which is disorienting. In field-centric control, pushing the joystick "away" always moves the robot down the field, no matter which way the chassis happens to be pointing. Drivers overwhelmingly prefer it, especially with swerve.

The mechanism is a single coordinate rotation. You take the driver's field-relative stick commands and rotate them into the robot frame using the current heading, then run inverse kinematics. WPILib gives you exactly this with ChassisSpeeds.fromFieldRelativeSpeeds():

ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
    vxFieldMetersPerSecond,   // stick "away from driver"
    vyFieldMetersPerSecond,   // stick "left"
    omegaRadiansPerSecond,    // rotation
    poseEstimator.getEstimatedPosition().getRotation()  // current heading
);
SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);

Because field-centric driving depends on heading being correct, it is only as good as your heading estimate. A drifting gyro slowly rotates the driver's whole frame of reference — "forward" starts pulling to one side. This is another reason to zero the gyro carefully and to keep it well-mounted and rigid. It's also why some teams periodically re-seed heading from a confident multi-tag vision solve. Note that fromFieldRelativeSpeeds expects the heading in the same field frame your poses live in; if your driver's "forward" and the field's +X don't line up (red vs. blue alliance), you handle that by offsetting the heading you pass in, not by rewriting the math.

Putting it together

A clean, robust localization stack looks like this:

1. Calibrate the distance-per-encoder-tick conversion empirically against a tape measure. This is your foundation.
2. Feed heading from a dedicated IMU (Pigeon 2, navX2, or similar), zeroed while the robot sits still at a known orientation. Let it finish booting untouched.
3. Run SwerveDrivePoseEstimator.update() every loop with gyro heading and module positions for a smooth, high-rate estimate.
4. Add vision measurements from AprilTags with accurate capture timestamps, distance-scaled standard deviations, and sanity checks that reject implausible poses.
5. Drive field-centric using the fused heading, and hand the fused pose to your path follower for autonomous.

That fused pose is what closes the loop for autonomous path following — the follower compares the estimated pose against the path it wants to be on and corrects the difference. For the trajectory side of that story, see autonomous with PathPlanner; for structuring all of this cleanly in code, command-based programming; and to validate the whole stack before you ever touch carpet, simulation with WPILib.

Frequently asked questions

Do I need vision and pose estimation, or is plain odometry enough?

For short, simple autonomous routines — drive forward, do one thing, stop — plain SwerveDriveOdometry is often fine, because drift doesn't have time to accumulate. The moment you want long multi-step autos, precise scoring alignment, or drive-to-point buttons in teleop, the drift becomes the limiting factor and you want a PoseEstimator fusing AprilTags. A good rule: if being off by 15–30 cm at the end of a routine would cost you, upgrade to pose estimation.

Why do I pass a timestamp to addVisionMeasurement instead of just the pose?

Because a camera measurement describes where the robot was when the image was captured, which is tens of milliseconds before your code receives it. The estimator uses that timestamp to rewind its internal history to the correct moment, blend the vision pose in there, and replay odometry forward — so the correction lands where it belongs instead of being smeared to "now." PhotonVision and Limelight both report capture timestamps for exactly this reason; passing the current time instead reintroduces the very latency error the filter exists to remove.

Should heading ever come from the wheels instead of the gyro?

No — always prefer the gyro. Wheel-derived heading integrates wheel slip and diameter error directly into your angle, and it drifts far faster than any modern IMU. WPILib's odometry and pose-estimator classes are built to take gyro heading as a direct input. The only time you lean on vision for heading is to occasionally re-seed a drifting gyro from a confident multi-tag solve, and even then you keep the vision heading standard deviation large so it corrects gently.

My robot's reported position is consistently off by a few percent. What's wrong?

Almost certainly your distance-per-tick conversion — the gear ratio or the wheel diameter. A consistent percentage error that scales with distance driven is the signature of a bad conversion factor, not slip (slip is random and worst under hard maneuvers). Drive a known distance against a tape measure, compare it to the reported odometry, and correct the ratio. Remember that swerve wheel tread wears down over a season, shrinking the effective diameter, so re-measure periodically.

How do I keep a bad AprilTag reading from teleporting my robot across the field?

Filter before the measurement reaches the estimator. Reject poses that land off the field, poses that jump an implausible distance from your current estimate in a single frame, and single-tag solves from far away. Then scale the vision standard deviations by distance so that even accepted far-away readings count for little. With sensible gating plus distance-scaled trust, a spurious detection nudges the estimate at most a centimeter or two instead of yanking it — and the next good frame washes it out.