Unit 7 · Lesson 4

Swerve Odometry

Kinematics tells the robot what to do. Odometry tells the robot where it is. These are the two halves of autonomous control — and without odometry, your robot drives blindly. This lesson builds the position-tracking layer that PathPlanner, field-oriented control, and vision fusion all depend on.

By the end of this lesson, you will:

Explain dead reckoning as the conceptual foundation of odometry
Describe a Pose2d and its three components: X position, Y position, and heading
Construct a SwerveDriveOdometry object with the correct initial pose and module states
Call update() every loop with current gyro angle and module states
Call resetPose() correctly in autonomousInit() and explain why placement matters
Use a Field2d widget to visualize the robot's estimated pose in Glass during simulation and testing

What is Odometry?

Odometry is the process of estimating a robot's position on the field using sensor data. It starts from a known position and accumulates movement — each 20 ms cycle, it asks: "given how fast and in what direction each wheel moved, where must the robot be now?" Then it updates the estimate.

This process is called dead reckoning — the same technique sailors used for centuries to estimate position at sea when they couldn't see the stars. You know where you started, you track every small movement, and you integrate them into a running position estimate. It accumulates error over time (small wheel slip compounds into positional drift), but over the 15-second autonomous period with well-calibrated hardware, it's accurate enough to reliably score game pieces.

Odometry does not require any external sensors — just the encoders on your drive motors and the gyro for heading. It's the baseline. In Lesson 10, SwerveDrivePoseEstimator adds vision measurements to correct that accumulated error.

Pose2d: The Robot's Address on the Field

A Pose2d is three numbers: X position (meters from the field origin), Y position (meters from the field origin), and a Rotation2d heading. Together they completely describe where the robot is and which direction it's facing.

The WPILib field coordinate system places the origin in the bottom-left corner of the blue alliance wall. Positive X runs toward the red alliance wall (downfield). Positive Y runs toward the left side of the field from the blue alliance's perspective. This is fixed — it doesn't change between seasons. Your robot's pose is always relative to this coordinate system.

💡 Gyro convention: make sure you're going the same direction as WPILib

WPILib's pose estimator expects counter-clockwise positive rotation (the mathematical convention). The Pigeon 2's yaw is clockwise positive by default. If you pass the raw Pigeon 2 yaw to odometry without negating it, the robot will appear to mirror its heading in odometry — it turns right but the odometry thinks it turned left. The fix: negate the yaw when constructing the Rotation2d: Rotation2d.fromDegrees(-m_pigeon.getYaw().getValueAsDouble()). This is one of the most common first-time swerve bugs.

Interactive Field Visualization

Click a scenario below to see what odometry is tracking — the robot's estimated pose as it executes an autonomous routine. This is what the Field2d widget in Glass shows in real time.

Pose2d tracking — simulated field view

← select a scenario to see the pose tracking visualization

The Code: Building and Updating Odometry

SwerveDrivetrain.java — odometry setup and update

// ── Odometry — position tracking ──────────────────────
private final SwerveDriveOdometry m_odometry;
private final Field2d m_field = new Field2d();

public SwerveDrivetrain() {
  // ... hardware initialization ...

  // Construct odometry with initial known state
  m_odometry = new SwerveDriveOdometry(
    m_kinematics,
    getHeading(),
    getModulePositions(),
    new Pose2d() // start at field origin
  );

  // Publish Field2d to SmartDashboard
  SmartDashboard.putData("Field", m_field);
}

@Override
public void periodic() {
  // Update pose estimate with latest sensors
  m_odometry.update(getHeading(), getModulePositions());

  // Visualize in Glass / Shuffleboard
  m_field.setRobotPose(m_odometry.getPoseMeters());
}

// ── Public API ────────────────────────────────────────
public Pose2d getPose() {
  return m_odometry.getPoseMeters();
}

public void resetPose(Pose2d pose) {
  m_odometry.resetPosition(
    getHeading(), getModulePositions(), pose);
}

private Rotation2d getHeading() {
  return Rotation2d.fromDegrees(
    -m_pigeon.getYaw().getValueAsDouble()); // negated!
}

private SwerveModulePosition[] getModulePositions() {
  return new SwerveModulePosition[] {
    m_frontLeft.getPosition(),
    m_frontRight.getPosition(),
    m_backLeft.getPosition(),
    m_backRight.getPosition()
  };
}

← click a highlighted token

SwerveModulePosition: What Odometry Reads

Odometry needs to know not just what each module is doing (a SwerveModuleState, used for control) but what it has done (a SwerveModulePosition, used for position tracking). The position includes:

distanceMeters — the total distance the wheel has traveled since the last reset, in meters
angle — the current steering angle as a Rotation2d

Your SwerveModule.getPosition() method reads the drive encoder's accumulated position (converted from rotations to meters using kDriveConversionFactor × kWheelCircumference) and the CANcoder's current absolute angle. This data, updated every 20 ms, feeds the forward kinematics inside SwerveDriveOdometry.update().

⚠️ resetPose() must be called in autonomousInit(), not the auto command

If your autonomous routine starts by resetting the pose, you need the reset to happen before the first periodic() loop of autonomous — not inside the first command that runs. Call drivetrain.resetPose(startingPose) in autonomousInit() in Robot.java, before scheduling the auto command. If the reset is inside the command's initialize(), the odometry has already run one bad update cycle with stale position data from the previous match. In PathPlanner, the library handles this automatically — but understanding the timing matters when something goes wrong.

Why Odometry Drifts — and What To Do About It

Odometry error accumulates from three sources:

Wheel slip — when the robot gets hit or accelerates hard, wheels briefly slip on carpet. The encoder counts rotations that didn't move the robot. A 15-second autonomous with aggressive defense can accumulate 5–15 cm of positional error from slip alone.
Gyro drift — MEMS gyroscopes accumulate small heading errors over time. Even the Pigeon 2, with its excellent temperature compensation, can drift 1–3° over a full match. In odometry, heading error compounds into positional error.
Encoder backlash — small amounts of gear backlash in the module gearbox cause the encoder to read slightly different values at the same physical position depending on approach direction.

For short auto routines (15 seconds, minimal defense contact), pure odometry is usually sufficient. For longer routines or high-accuracy scoring, SwerveDrivePoseEstimator (Lesson 10) fuses odometry with AprilTag vision measurements to continuously correct accumulated error.

🔍 LRI Perspective: "Odometry is a budget. Spend it wisely."

Think of odometry accuracy as a budget that depletes over the autonomous period. Aggressive acceleration depletes it faster (wheel slip). Defense contact depletes it instantly. The teams with the most reliable autonomous routines design their paths to reset the odometry budget: they path to a known AprilTag, use the vision fix to re-anchor the pose, then path to the scoring location from a known-good position. Pure dead reckoning from the start to the shot, 5 meters and two turns later, is spending the entire budget on one bet. Most teams who lose autonomous matches were betting their entire odometry budget on perfect field conditions. Use vision to reload it.

🔌 System Check — Odometry Verification

Verify odometry before autonomous testing:

Open Glass (WPILib Simulate or Field2d widget in Shuffleboard). Drive the robot a known distance — 1 meter forward. Confirm the Field2d widget shows the robot at approximately (1.0, 0.0) relative to its start. If it shows significantly more or less than 1.0m, your kDriveConversionFactor constant is wrong — recheck the gear ratio and wheel circumference.
Drive the robot in a 1-meter square and return to the start. Confirm the robot's pose on Field2d is within 5 cm of the starting position. Larger errors indicate accumulated heading or wheel slip problems. If heading is drifting, verify the Pigeon 2 yaw negation is applied.
In autonomousInit(), add a debug line: SmartDashboard.putString("Auto/StartPose", drivetrain.getPose().toString()). Verify the reported start pose matches your expected starting position after calling resetPose(). If it doesn't, the reset is not being applied before the first update.

Knowledge Check

1. You call resetPose(new Pose2d(1.5, 4.0, Rotation2d.fromDegrees(0))) in autonomousInit(). What is the robot telling the odometry system?

A The robot should drive to position (1.5, 4.0) and then face 0°.
B "I am currently at X=1.5m, Y=4.0m on the field, facing 0° (toward the red alliance wall). All future position estimates should accumulate from this known starting point."
C The robot resets its drive encoder positions to zero and sets the gyro heading to 0°.
D The robot's maximum speed is limited to 1.5 m/s for the next 4.0 seconds.

2. Your robot drives a straight 2-meter line in autonomous but Field2d shows a curved arc of about 2.3 meters. What is the most likely cause?

A The drive encoder conversion factor is too large, overreporting distance.
B The Pigeon 2 yaw was not negated when constructing the heading, so the odometry's heading is mirrored — the robot turns right but odometry thinks it turned left, curving the estimated trajectory in the wrong direction.
C The wheelbase constant is incorrect, causing kinematics to compute wrong module positions.
D resetPose() was called with the wrong starting position.

3. SwerveModulePosition has both a distanceMeters and an angle field. Why does odometry need the steering angle from each module, not just the drive distance?

A The steering angle is used to calculate the drive conversion factor at runtime.
B Distance alone only tells you how far each wheel moved — not in which direction. The steering angle tells odometry the direction that travel contributed to. A wheel traveling 0.5 meters at 90° (sideways) contributes entirely to Y position; the same distance at 0° contributes entirely to X. Both together give the full displacement vector.
C The steering angle is used to verify that the module was not stalled during the last update cycle.
D The gyro provides the overall heading, so the module angle is only used as a redundant backup sensor.

💪 Practice Prompt

Add Odometry and Field2d to Your Drivetrain

Add a SwerveDriveOdometry field and a Field2d field to your SwerveDrivetrain subsystem. Construct the odometry in the constructor with your kinematics object, the heading from getHeading() (negated Pigeon 2 yaw), and the module positions from getModulePositions(). Start at the origin (new Pose2d()).
In periodic(), call m_odometry.update(getHeading(), getModulePositions()) and then m_field.setRobotPose(m_odometry.getPoseMeters()). Call SmartDashboard.putData("Field", m_field) in the constructor. Open Glass in simulation and confirm the Field2d widget appears.
Write getPosition() in your SwerveModule class that returns a SwerveModulePosition with drive distance (encoder rotations × wheel circumference) and current CANcoder angle. Call this from the drivetrain's getModulePositions() array.
Write a resetPose(Pose2d pose) method in the drivetrain. Add a call to it in Robot.java autonomousInit() with your expected starting position. Run the robot (or simulator) through a 1-meter drive and verify the Field2d widget shows the correct displacement.
Stretch goal: Drive the robot in a square (forward 1m, strafe right 1m, backward 1m, strafe left 1m) and observe the Field2d trail. The pose should return close to the origin. Measure the error and record it. Try again after verifying your conversion factors. This square test is a standard odometry calibration exercise — it's the most efficient way to expose gear ratio or wheelbase errors before autonomous testing.