Odometry

An autonomous robot needs an estimate of where it is. Odometry builds that estimate by measuring how the robot moved since the previous update. Each update adds the measured movement to the previous pose estimate.

Small measurement errors add up over a route, and wheel slip can make the estimate wrong even when the code is correct.

For the coordinate examples below, use the lower-left as the origin, +X to the right, and +Y toward the top of the field. This is the convention used in this guide; libraries may choose different axes.

Odometry and localization

Localization is the broader task of estimating the robot's field position and heading (its pose). Odometry estimates changes in that pose by integrating measured robot movement. An encoder is only a sensor: it reports shaft rotation. It does not know the robot's field position by itself.

You have already seen drive encoders used to estimate how far a movement command travels in Time vs Encoder-Based Movement. A localization update can use those same measurements to maintain an ongoing pose estimate. The common setups below use different sensors and make different tradeoffs.

Drive-motor encoders

Drive-motor encoders are attached to the wheels that propel the robot. On a differential (tank) drive, the left and right wheel changes can estimate forward movement and turning. Software can integrate those changes, together with a starting pose and heading, into an encoder-based localization estimate. The encoder hardware alone is not odometry or localization; the movement model and update code produce the estimate. Teams also commonly call this method drive-encoder odometry.

This is a useful starting point because it needs no extra tracking wheels. It is most informative for forward and backward travel on a tank drive. Wheel slip changes the relationship between encoder rotation and floor movement, and two drive encoders do not measure sideways travel. Mecanum robots need a model that also accounts for sideways wheel motion, which is still vulnerable to drive-wheel slip.

Dead-wheel odometry

Dead wheels (also called tracking wheels or odometry pods) are unpowered wheels that roll against the floor and turn their own encoders. Since they do not propel the robot, their readings are less affected by drive-wheel slip. Their positions on the robot matter: the update uses wheel direction and distance from the robot's center to separate forward, sideways, and rotational movement.

A common three-wheel layout uses two parallel wheels to measure forward travel and turning, plus one perpendicular wheel to measure sideways travel. A two-wheel layout uses one parallel and one perpendicular tracking wheel, with an IMU supplying heading. Both need careful mounting and calibration; the three-wheel setup uses another encoder channel, while the two-wheel setup depends more on its heading measurement. Dead wheels can still slip, lift off the floor, or give a biased estimate if their offsets and scales are wrong.

IMU-assisted odometry

An IMU measures orientation changes; it does not measure field X/Y position. An odometry system can use IMU yaw for heading while wheel encoders measure translation. This can reduce heading error from wheel-based turn estimates, but it cannot correct a translation error caused by slipping wheels. IMU yaw also needs a known starting reference and can drift or be affected by mounting and configuration.

An IMU can be paired with drive encoders or dead wheels. A camera or another known field reference can add position measurements and correct accumulated drift; that combination is covered in Sensor Fusion.

Encoder scale

For a drive-motor encoder, measure or calculate these values for your exact motor, gearing, and drive wheel. For a tracking wheel, use that pod's encoder and wheel instead; include any gearing between them:

1. Encoder counts per motor-shaft revolution, from the motor or encoder documentation.
2. The number of motor-shaft turns for one wheel turn (the reduction ratio, not its inverse).
3. The wheel's effective diameter while loaded on the floor.

Then:

counts per wheel revolution = encoder counts per motor revolution × gear ratio
wheel distance per revolution = π × effective wheel diameter
counts per inch = counts per wheel revolution ÷ wheel distance per revolution
distance moved = encoder count change ÷ counts per inch

Do not copy a countsPerInch value from another robot. Different motors, encoders, gears, wheel compression, and mounting change it. A practical check is to roll the robot a measured distance and compare it with the encoder estimate.

Encoder deltas

At the start, save the current encoder readings as a baseline. Each loop, subtract the previous readings from the new ones. Those differences are the movement since the last update. Then save the new readings for the next loop.

This is the small idea inside an odometry update:

int encoderChange = currentCount - previousCount;
double distanceChange = encoderChange / countsPerInch;
previousCount = currentCount;

val encoderChange = currentCount - previousCount
val distanceChange = encoderChange / countsPerInch
previousCount = currentCount

Here, countsPerInch must be the value calibrated for your robot, and previousCount must be updated every loop. This snippet converts one encoder's change to a distance; it is not a full position tracker.

Field coordinates

Encoders measure movement relative to the robot. Field coordinates are fixed to the playing field. To update field position, the program needs the robot's heading as well as its forward and sideways movement.

For a simple straight movement, the idea is:

field X change = forward distance × cos(heading)
field Y change = forward distance × sin(heading)

That example assumes the chosen coordinate convention and a straight segment. A real update must also account for turns during the loop, sideways movement for a holonomic robot, and the orientation of each encoder. Use one convention consistently; FTC libraries may define axes and headings differently.

Here is a working first pose update for encoder-based localization on a differential-drive robot, with left and right drive encoders plus an IMU. The encoders are sensors, not a localization system on their own; this code integrates their movement into a pose estimate. This is commonly called drive-encoder odometry, and it is not a dead-wheel setup. It assumes COUNTS_PER_INCH has been measured and the IMU has been initialized for the robot's actual mounting orientation. The example uses field +X forward when heading is zero and +Y to the left.

public final class DifferentialOdometry {
    private final double countsPerInch;
    private int previousLeft;
    private int previousRight;
    private boolean initialized;
    private double x;
    private double y;
    private final double startHeadingRadians;
    private double initialRawYaw;

    public DifferentialOdometry(double countsPerInch, double startX, double startY,
                                double startHeadingRadians) {
        if (!Double.isFinite(countsPerInch) || countsPerInch <= 0.0
                || !Double.isFinite(startX) || !Double.isFinite(startY)
                || !Double.isFinite(startHeadingRadians)) {
            throw new IllegalArgumentException("Finite pose and positive scale required");
        }
        this.startHeadingRadians = startHeadingRadians;
        this.countsPerInch = countsPerInch;
        this.x = startX;
        this.y = startY;
    }

    public void update(int leftTicks, int rightTicks, double headingRadians) {
        if (!Double.isFinite(headingRadians)) throw new IllegalArgumentException("Finite yaw required");
        if (!initialized) {
            previousLeft = leftTicks;
            previousRight = rightTicks;
            initialRawYaw = headingRadians;
            initialized = true; // First call records encoder and yaw baselines.
            return;
        }

        double left = (leftTicks - previousLeft) / countsPerInch;
        double right = (rightTicks - previousRight) / countsPerInch;
        previousLeft = leftTicks;
        previousRight = rightTicks;

        double forward = (left + right) / 2.0;
        double fieldHeading = startHeadingRadians
            + Math.atan2(Math.sin(headingRadians - initialRawYaw),
                         Math.cos(headingRadians - initialRawYaw));
        x += forward * Math.cos(fieldHeading);
        y += forward * Math.sin(fieldHeading);
    }

    public double x() { return x; }
    public double y() { return y; }
}

import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.sin

class DifferentialOdometry(
    private val countsPerInch: Double,
    startX: Double,
    startY: Double,
    private val startHeadingRadians: Double
) {
    init {
        require(countsPerInch.isFinite() && countsPerInch > 0.0)
        require(startX.isFinite() && startY.isFinite() && startHeadingRadians.isFinite())
    }
    private var initialRawYaw = 0.0
    private var previousLeft = 0
    private var previousRight = 0
    private var initialized = false
    private var x = startX
    private var y = startY

    fun update(leftTicks: Int, rightTicks: Int, headingRadians: Double) {
        require(headingRadians.isFinite())
        if (!initialized) {
            previousLeft = leftTicks
            previousRight = rightTicks
            initialRawYaw = headingRadians
            initialized = true // First call records encoder and yaw baselines.
            return
        }

        val left = (leftTicks - previousLeft) / countsPerInch
        val right = (rightTicks - previousRight) / countsPerInch
        previousLeft = leftTicks
        previousRight = rightTicks

        val forward = (left + right) / 2.0
        val fieldHeading = startHeadingRadians +
            atan2(sin(headingRadians - initialRawYaw), cos(headingRadians - initialRawYaw))
        x += forward * cos(fieldHeading)
        y += forward * sin(fieldHeading)
    }

    fun x() = x
    fun y() = y
}

Create this object with a measured starting pose, including field heading: for example new DifferentialOdometry(100.0, 0.0, 0.0, Math.PI / 2.0) in Java, or DifferentialOdometry(100.0, 0.0, 0.0, kotlin.math.PI / 2.0) in Kotlin, for a robot facing field +Y. The scale 100 is an example; use your measured scale.

Call update() once before movement to record the starting encoders and raw yaw. IMU yaw can carry over between OpModes, so initialising the IMU does not necessarily align it with field +X. This helper subtracts the starting yaw and adds your chosen field heading. To reset pose, create a new estimate and record all starting readings together.

In the OpMode loop, pass the latest encoder counts and yaw in radians, then display the estimate:

double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
odometry.update(leftMotor.getCurrentPosition(), rightMotor.getCurrentPosition(), heading);
telemetry.addData("Estimated X (in)", "%.1f", odometry.x());
telemetry.addData("Estimated Y (in)", "%.1f", odometry.y());
telemetry.update();

val heading = imu.robotYawPitchRollAngles.getYaw(AngleUnit.RADIANS)
odometry.update(leftMotor.currentPosition, rightMotor.currentPosition, heading)
telemetry.addData("Estimated X (in)", "%.1f", odometry.x())
telemetry.addData("Estimated Y (in)", "%.1f", odometry.y())
telemetry.update()

This simple version uses the current heading for each small movement. It is a good first exercise for straight segments; the Sensor Fusion example adds an encoder-based heading change and a mid-step heading estimate. Do not apply this two-encoder translation to a mecanum robot's sideways motion.

Calibration

Keep the robot on blocks for direction checks, then test at low speed in a clear area:

1. Reset and baseline: set the starting pose and record encoder readings before movement.
2. Drive straight: command a measured distance, such as 24 inches. Compare the estimated distance with a tape measure.
3. Drive sideways: if the robot can strafe, test that direction separately. Sideways motion is not measured correctly by a two-wheel tank-drive calculation alone.
4. Turn in place: compare the estimated heading change with a measured quarter-turn. Check the sign and units (degrees or radians).
5. Repeat: adjust only the relevant scale or geometry value, then run the same test again.

If the error grows more over a longer test, check the scale. If it changes unpredictably, inspect wheel slip, loose mounts, encoder direction, and heading readings before changing math.

Setup comparison

These are starting points, not accuracy guarantees. The best setup depends on drivetrain, mounting, budget, and the route your autonomous needs to run.

Localization libraries

Maintaining robot pose requires careful coordinate math, encoder calibration, heading handling, and drift correction. For a robot that needs to follow field paths, use a current localization/path-following library such as Pedro Pathing, and follow the setup guide for your installed version. Learn the measurements in this page so you can understand its telemetry and tune it.

Odometry alone drifts. Combining it with a known field target or another independent measurement can correct that drift; see Sensor Fusion.

If measured pushes or turns disagree with telemetry, use Localization troubleshooting to separate scale, offset, heading, update, and mounting problems before tuning the follower.

Common checks

• Encoder direction agrees with the wheel's direction of travel.
• Starting encoder values are saved before the first movement update.
• Distance scale comes from your hardware and a measured test.
• Heading units and coordinate axes are consistent.
• A missing or invalid sensor reading does not command unexpected movement.
• Autonomous loops stop when the OpMode is stopped.

Continue with Motion Planning to decide where to drive, or Pure Pursuit to learn a path-following idea.