Chapter 5: Odometry — Dead Reckoning and Its Limits

Odometry is the process of estimating a robot’s pose (position and orientation) by integrating wheel motion over time. It is analogous to a sailor’s dead reckoning: knowing speed and heading, you integrate forward to get position. It requires no external reference — no GPS, no landmarks, no camera — which makes it available everywhere. It is also the primary source of drift in mobile robot navigation.

5.1 Wheel Encoder Types

An encoder converts rotational motion into a digital signal that can be counted.

Optical incremental encoder: a disc with alternating transparent and opaque slots rotates past an optical sensor (LED + phototransistor pair). Each edge generates a pulse. Counting pulses gives distance; measuring pulse rate gives speed. A typical low-cost encoder on an N20 motor produces 20–40 pulses per revolution on the motor shaft. With a 30:1 gear reduction, this becomes 600–1200 pulses per wheel revolution — sufficient for reasonable odometry.

Quadrature encoding: two optical sensors are mounted 90° apart (in phase). This produces two signals, A and B, that are 90° out of phase. The sequence of A and B edges determines both count and direction:

Quadrature encoders give four counts per mechanical slot (4× resolution) and unambiguous direction detection.

CPR (Counts Per Revolution) math: with a motor encoder CPR of 40 and gear ratio of 30:1, the wheel encoder CPR is $40 \times 30 = 1200$. With wheel radius $r = 0.033$ m, the distance per count is:

$d_{count} = 2\pi r / CPR = 2\pi \times 0.033 / 1200 \approx 0.000173$ m/count

That is about 0.17 mm per count — fine enough for smooth odometry.

5.2 Odometry Equations

Given left and right encoder tick counts $\Delta n_L$ and $\Delta n_R$ during time step $\Delta t$:

  1. Convert ticks to distances: $\Delta d_L = \Delta n_L \cdot d_{count}$, $\Delta d_R = \Delta n_R \cdot d_{count}$
  2. Compute linear and angular displacement: $\Delta d = (\Delta d_R + \Delta d_L)/2$, $\quad \Delta\theta = (\Delta d_R - \Delta d_L)/L$
  3. Update pose (using the midpoint integration formula for better accuracy): $\theta_{new} = \theta + \Delta\theta$ $x_{new} = x + \Delta d \cdot \cos(\theta + \Delta\theta/2)$ $y_{new} = y + \Delta d \cdot \sin(\theta + \Delta\theta/2)$

The midpoint formula (using $\theta + \Delta\theta/2$ rather than $\theta$) reduces numerical integration error for curved paths.

See code/05_odometry.py

def update(self, ticks_left: int, ticks_right: int) -> tuple:
    dl = ticks_left  * self.dist_per_tick
    dr = ticks_right * self.dist_per_tick
    dd = (dr + dl) / 2.0
    dtheta = (dr - dl) / self.track_width
    mid_theta = self.theta + dtheta / 2.0
    self.x     += dd * math.cos(mid_theta)
    self.y     += dd * math.sin(mid_theta)
    self.theta += dtheta
    self.theta  = math.atan2(math.sin(self.theta),
                              math.cos(self.theta))
    return self.x, self.y, self.theta

The final line normalises the angle to $[-\pi, \pi]$ using the atan2 trick, preventing floating-point overflow in long runs.

5.3 Error Sources

Odometry error accumulates for several reasons:

Wheel slip. On hard, smooth floors, friction is usually sufficient. On loose carpet, tile transitions, or inclines, wheels slip — the motor turns but the robot does not move the expected distance. Slip cannot be detected from encoders alone.

Uneven terrain. A slight floor slope or a cable crossing causes one wheel to move a different effective radius. Over many metres this produces a systematic heading drift.

Encoder quantization. With 1200 CPR, each count represents 0.17 mm. At 0.3 m/s, the robot travels 0.17 mm between counts, so update rates above ~1760 Hz are meaningless. In practice, 50–100 Hz is fine.

Float accumulation. Python floats have about 15 significant digits of precision. Over thousands of update steps, rounding errors are negligible for robot-scale operation (rooms of tens of metres). This is not a practical concern.

Wheel radius error. If the nominal wheel radius is 0.033 m but the actual radius is 0.0325 m (perhaps due to tyre compression), every distance is off by 1.5%. Over 10 m, this is 15 cm.

5.4 Representing Pose Uncertainty

Odometry gives a point estimate of pose. In reality, the true pose is uncertain. If we model the uncertainty as a Gaussian, the 2-D position uncertainty is an error ellipse. For a robot driving straight, the uncertainty grows primarily along the heading direction. For a robot turning, angular uncertainty causes the ellipse to fan out transversely.

A simple model: for each metre driven, add $\sigma_{trans} = 0.02$ m of translational uncertainty and $\sigma_{rot} = 2°$ of rotational uncertainty. After 5 m, the positional uncertainty is roughly $\pm 10$ cm and the heading uncertainty is $\pm 10°$. These errors are too large for precise docking manoeuvres, but adequate for room-scale navigation with occasional landmark corrections.

Chapter 12 discusses occupancy grid mapping and SLAM, which use sensor data to correct accumulated odometry drift.

5.5 Practical Exercise: Drive a Square

A classic test of odometry accuracy is to drive a square — four sides of equal length, four 90° turns — and measure the start/end error.

The pose-logging script drives the square and plots the estimated trajectory:

See code/05_pose_logger.py

A well-calibrated robot on a hard floor returns to within 3–5 cm and 3–5° of its starting pose after a 1 m square. Common failure modes:

Each failure mode points to a specific calibration parameter to adjust.

5.6 When Odometry Is Good Enough

For tasks with short-range navigation requirements (< 5 m), hard floors, and no precise docking, odometry alone is often sufficient. Line-following (Chapter 6) provides a complementary approach for structured environments: instead of accumulating pose, the robot follows a physical guide on the floor, making odometry drift irrelevant.

For longer-range autonomous navigation in unstructured environments, odometry must be augmented with landmark-based localisation, camera-based visual odometry, or the SLAM techniques of Chapter 12.


Navigation: