Kinematics and Odometry: Knowing Where You Are

The coordinate system

WPILib uses a field coordinate system with Pose2d (an x, y position plus a heading Rotation2d). Distances are in meters and angles in radians (WPILib’s units library helps keep these straight). Knowing your pose lets autonomous code drive to places, not just for a time.

Kinematics: wheels ↔ robot

Kinematics classes convert between individual wheel states and overall robot motion (a ChassisSpeeds of forward, sideways, and rotational velocity):

• DifferentialDriveKinematics — for tank/west-coast drives (left and right wheel speeds).
• SwerveDriveKinematics — for swerve drives (each module’s speed and angle).
• MecanumDriveKinematics — for mecanum drives.

For swerve, kinematics turns a desired ChassisSpeeds into the per-module SwerveModuleStates your code commands.

Odometry: integrating motion into a pose

Odometry tracks the robot’s pose by continuously integrating wheel encoder distances and the gyro heading. WPILib provides:

• DifferentialDriveOdometry — needs the gyro angle (Rotation2d) and left/right encoder distances; optional starting Pose2d.
• SwerveDriveOdometry — needs the kinematics, gyro angle, and module positions.
• MecanumDriveOdometry.

You call odometry.update(...) every loop (typically in the subsystem’s periodic()):

@Override
public void periodic() {
  m_pose = m_odometry.update(
      m_gyro.getRotation2d(),
      m_leftEncoder.getDistance(),
      m_rightEncoder.getDistance());
}

Why a gyro is essential

Heading drift from wheels alone is severe. A dedicated gyro — e.g., a navX2 (Studica) or a CTRE Pigeon 2.0 (the Pigeon2 class in Phoenix 6) — gives an accurate heading, which odometry relies on. Wheels estimate distance; the gyro estimates angle.

Pose estimators: fusing vision

Plain odometry slowly drifts because of wheel slip and encoder error. WPILib’s pose estimators — DifferentialDrivePoseEstimator, SwerveDrivePoseEstimator, and MecanumDrivePoseEstimator — are drop-in upgrades that also fuse latency-compensated vision measurements (e.g., AprilTag detections from PhotonVision or Limelight) with encoder/gyro data via addVisionMeasurement(...). They correct drift and handle noisy vision gracefully, giving a far more accurate field pose. Most competitive teams use a pose estimator rather than raw odometry.

How this enables autonomous

Once you reliably know your pose, you can do real navigation: “drive to (3.5 m, 2.0 m) facing 90°.” That’s exactly what trajectory followers consume — they compare your odometry pose to a planned path and command speeds to stay on it. Accurate odometry is the foundation everything else in this module stands on.

Key takeaways

• Pose2d (x, y, heading) in meters and radians describes the robot on the field.
• Kinematics convert between wheel states and ChassisSpeeds (differential, swerve, mecanum).
• Odometry integrates encoder distances + gyro heading into a pose; update it every loop.
• A dedicated gyro (navX2 or CTRE Pigeon 2.0 / Pigeon2) is essential for accurate heading.
• Pose estimators upgrade odometry by fusing AprilTag vision (PhotonVision/Limelight) to correct drift.

This lesson was adapted from learnfrc.com.