Encoder Odometry Math

name: encoder-odometry-math description: 'Compute odometry from wheel encoders. Use when implementing quadrature decoding, differential drive kinematics, or pose integration.'

Computing Odometry from Quadrature Encoders

Quadrature Encoding Fundamentals

A quadrature encoder outputs two square-wave signals (channel A and channel B) offset by 90°. The phase relationship between A and B determines direction. Counting modes:

• 1x counting: Count rising edges of channel A only → counts_per_rev = PPR (pulses per revolution)
• 2x counting: Count both edges of channel A → counts_per_rev = 2 × PPR
• 4x counting: Count both edges of BOTH channels → counts_per_rev = 4 × PPR

Always use 4x counting for maximum resolution. With a 64 PPR encoder and a 48:1 gear ratio:

counts_per_rev = 4 × 64 × 48 = 12,288 counts/revolution

Distance Per Count

Given wheel diameter $d_w$ (in meters):

$$ \text{distance_per_count} = \frac{\pi \cdot d_w}{\text{counts_per_rev}} $$

Example: 90mm wheel, 12,288 counts/rev:

$$ \text{distance_per_count} = \frac{\pi \times 0.090}{12288} = 2.30 \times 10^{-5} \text{ m/count} \approx 0.023 \text{ mm/count} $$

Wheel Velocities

Read encoder counts at a fixed interval $\Delta t$ (typically 10-20ms). Compute velocity for each wheel:

$$ v_{\text{wheel}} = \frac{\Delta \text{counts}}{\Delta t} \times \text{distance_per_count} $$

Where $\Delta \text{counts} = \text{current_counts} - \text{previous_counts}$.

Handle encoder overflow: if using signed 32-bit integers, the count wraps at $\pm 2^{31}$. With 12,288 counts/rev and a wheel doing 5 rev/s, you get 61,440 counts/s. Time to overflow: $2^{31} / 61440 \approx 34,952$ seconds (~9.7 hours). For long-running robots, compute deltas (not absolute positions) and reset periodically, or use 64-bit counters.

Differential Drive Forward Kinematics

For a two-wheeled differential drive robot with wheel separation $L$ (distance between wheel contact points, center-to-center):

$$ v = \frac{v_R + v_L}{2} $$

$$ \omega = \frac{v_R - v_L}{L} $$

Where:

• $v$ = linear velocity of the robot center (m/s)
• $\omega$ = angular velocity about the robot center (rad/s)
• $v_R, v_L$ = right and left wheel velocities (m/s)
• $L$ = wheel separation (m)

Pose Integration: Euler Method

Given current pose $(x, y, \theta)$ and computed $(v, \omega)$:

$$ \theta_{t+1} = \theta_t + \omega \cdot \Delta t $$

$$ x_{t+1} = x_t + v \cdot \cos(\theta_t) \cdot \Delta t $$

$$ y_{t+1} = y_t + v \cdot \sin(\theta_t) \cdot \Delta t $$

This is a first-order approximation. It works well when $\omega \cdot \Delta t$ is small (high update rate, gentle turns).

Pose Integration: Runge-Kutta (Second-Order)

For better accuracy during turns, use the midpoint method. Compute the heading at the midpoint of the arc:

$$ \theta_{\text{mid}} = \theta_t + \frac{\omega \cdot \Delta t}{2} $$

$$ x_{t+1} = x_t + v \cdot \cos(\theta_{\text{mid}}) \cdot \Delta t $$

$$ y_{t+1} = y_t + v \cdot \sin(\theta_{\text{mid}}) \cdot \Delta t $$

$$ \theta_{t+1} = \theta_t + \omega \cdot \Delta t $$

This significantly reduces integration error during arcs and S-curves.

Exact Arc Integration

When $\omega \neq 0$, the robot traces a circular arc of radius $R = v / \omega$:

$$ x_{t+1} = x_t + R \cdot [\sin(\theta_t + \omega \Delta t) - \sin(\theta_t)] $$

$$ y_{t+1} = y_t - R \cdot [\cos(\theta_t + \omega \Delta t) - \cos(\theta_t)] $$

$$ \theta_{t+1} = \theta_t + \omega \cdot \Delta t $$

When $\omega \approx 0$ (straight line), fall back to Euler to avoid division by zero.

Covariance Estimation

The nav_msgs/Odometry message requires a 6x6 covariance matrix for both pose and twist. For a differential drive encoder-only odometry:

# Pose covariance (x, y, z, roll, pitch, yaw) - only x, y, yaw are relevant
pose_cov = [
    0.01,  0.0,  0.0,  0.0,  0.0,  0.0,   # x variance
    0.0,   0.01, 0.0,  0.0,  0.0,  0.0,    # y variance
    0.0,   0.0,  1e6,  0.0,  0.0,  0.0,    # z (set large = unknown)
    0.0,   0.0,  0.0,  1e6,  0.0,  0.0,    # roll (unknown)
    0.0,   0.0,  0.0,  0.0,  1e6,  0.0,    # pitch (unknown)
    0.0,   0.0,  0.0,  0.0,  0.0,  0.03,   # yaw variance
]

Covariance should increase with speed and angular rate—encoders are less accurate when slipping at high speeds or during sharp turns. A simple model:

$$ \sigma_x^2 = k_1 \cdot |v| \cdot \Delta t + k_2 $$

$$ \sigma_\theta^2 = k_3 \cdot |\omega| \cdot \Delta t + k_4 $$

Where $k_1$ through $k_4$ are tuning constants determined experimentally.

Publishing Odometry in ROS 2

The odometry node must publish:

1. nav_msgs/Odometry on /odom — pose + twist with covariances
2. TF transform from odom → base_link (or base_footprint)

Both must be published at the same rate (typically 50-100 Hz) and with the same timestamp.

Common Problems and Solutions

Calibration Procedure

1. Wheel diameter: Manually push the robot exactly 2.000 m in a straight line. Record encoder counts. Compute actual diameter: $d = \frac{2.000 \times \text{counts_per_rev}}{\pi \times \Delta\text{counts}}$.
2. Wheel separation: Command the robot to rotate exactly 360° in place (use IMU or visual reference). Compute $L = \frac{(v_R - v_L) \times t}{2\pi}$, or equivalently $L = \frac{\Delta d_R - \Delta d_L}{2\pi}$ where $\Delta d$ is the arc length traveled.
3. Repeat 5 times, average the results. Mechanical compliance (tire squish) and surface friction affect these values.