M5-T2 — Odometry and Velocity Motion Models

Learning objectives

After this lesson, you should be able to:

• Write the odometry motion model (rotate, translate, rotate) with its four noise parameters, and sample from it.
• Write the velocity motion model as a circular arc, including its straight-line limit.
• Derive the Jacobians of the odometry model and propagate a pose covariance through a step.
• Explain why the sideways error grows like d^1.5, and when the first-order covariance underestimates the along-track error.

The intuition

A differential-drive robot knows how far each wheel has turned. Between two moments, its pose changes from $(x, y, \theta)$ to $(x', y', \theta')$. Any such change can be described as three simple movements: turn to face the new position ($\delta_{\text{rot1}}$), drive straight to it ($\delta_{\text{trans}}$), and turn to the new heading ($\delta_{\text{rot2}}$). Those three numbers are the odometry reading, and the odometry motion model takes them as the control.

Each of the three has an error, and the errors have simple physical origins.

• A turn is wrong by a small angle, because the wheels slip and the wheel base is not known exactly. The bigger the turn, and the farther the robot has travelled, the larger the angle error.
• A distance is wrong by a few percent, because the wheel diameter is not exact and the floor is not perfectly grippy. The error is proportional to the distance.

Four parameters $\alpha_1, \dots, \alpha_4$ say how much of each (the table below), and they are the only things to tune.

The picture is the most important thing to take from this lesson. A tiny heading error, a degree or so, is harmless at the start of a drive and ruinous at the end: the robot points slightly the wrong way and keeps driving, so the sideways error is roughly distance times heading error. And the heading error itself accumulates, as a random walk. The two effects multiply: the sideways error grows not like the distance, nor like its square root, but like $d^{3/2}$. After 20 m with each metre carrying 1° of heading noise, the robot is 1.46 m off to the side but only about 0.1 m off along the path. This is why the cloud of possible poses is a curved band, often called the banana, not a circle, and why a filter that describes the pose by independent x and y variances is badly wrong.

The odometry model has a rotate-drive-rotate structure with noise that grows with the size of each movement. Heading noise turns into sideways error, which grows faster than the distance.

Definitions

Odometry $u = (\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})$

The reported motion between two poses as turn, drive and turn. It is computed from the encoder readings of the two moments.

Noise parameters $\alpha_1, \dots, \alpha_4$

Non-negative numbers (units: radians² per radian², radians² per m², m² per m², m² per radian²) that set the variance of each movement.

Velocity control $(v, \omega)$

A commanded forward speed and turn rate, applied for a time $\Delta t$. Used when no odometry is available.

Jacobians $F, G$

$F$ is the derivative of the new pose with respect to the old pose, $G$ with respect to the control, both at the nominal values.

Derivation: the Jacobians and the d^1.5 law

The Jacobians

Write $\phi = \theta + \delta_{\text{rot1}}$ for the direction of travel. The motion function is $g(x, y, \theta; \delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}}) = (x + \delta_{\text{trans}}\cos\phi,\; y + \delta_{\text{trans}}\sin\phi,\; \theta + \delta_{\text{rot1}} + \delta_{\text{rot2}})$.

1. $$F = \dfrac{\partial g}{\partial (x, y, \theta)} = \begin{pmatrix} 1 & 0 & -\delta_{\text{trans}}\sin\phi \\ 0 & 1 & \delta_{\text{trans}}\cos\phi \\ 0 & 0 & 1 \end{pmatrix}$$

   Differentiate each row with respect to $x, y, \theta$ (note $\partial \phi/\partial\theta = 1$). The third column says that an error in the heading of the old pose moves the new position sideways by the distance travelled.

2. $$G = \dfrac{\partial g}{\partial (\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})} = \begin{pmatrix} -\delta_{\text{trans}}\sin\phi & \cos\phi & 0 \\ \delta_{\text{trans}}\cos\phi & \sin\phi & 0 \\ 1 & 0 & 1 \end{pmatrix}$$

   Differentiate with respect to the three control components. The first column is the same sideways push (the first turn changes the direction of travel), the second is the direction of travel itself, the third shows that the last turn changes only the heading.

3. $$\Sigma' = F\,\Sigma\,F^{\top} + G\,M\,G^{\top}$$

   Apply the first-order propagation of the last module, with independent control noise $M$.

Why sideways error grows like d^1.5

Let the robot drive $n$ steps of length $\ell$ straight ahead, $d = n\ell$. In each step the two turns contribute heading noise of variance $s^2 = 2\alpha_2\ell^2$ (using $\sigma^2_{\text{rot}} = \alpha_2\ell^2$ for each of the two turns).

1. $$\theta_k = \sum_{j=1}^{k} \varepsilon_j, \qquad \mathrm{Var}(\theta_k) = k\,s^2$$

   The heading error after $k$ steps is a random walk: independent increments $\varepsilon_j$ of variance $s^2$ add up in variance (the variance lesson).

2. $$y_n \approx \ell\sum_{k=1}^{n}\sin\theta_k \approx \ell\sum_{k=1}^{n}\theta_k = \ell\sum_{j=1}^{n} (n - j + 1)\,\varepsilon_j$$

   The sideways position is the sum of the sideways components of each step, $\ell\sin\theta_k$, and for small angles $\sin\theta \approx \theta$. Exchanging the order of the double sum shows that heading noise from step $j$ is felt by all the later steps.

3. $$\mathrm{Var}(y_n) = \ell^2 s^2\sum_{m=1}^{n} m^2 = \ell^2 s^2\,\dfrac{n(n+1)(2n+1)}{6} \approx \ell^2 s^2\,\dfrac{n^3}{3}$$

   The variances of independent terms add; the sum of squares formula for $\sum m^2$ gives $n^3/3$ for large $n$.

4. $$\mathrm{Var}(y) \approx \dfrac{2\alpha_2}{3}\,\ell\,d^{3}, \qquad \sigma_y \approx \sqrt{\dfrac{2\alpha_2\,\ell}{3}}\; d^{3/2}$$

   Substitute $s^2 = 2\alpha_2\ell^2$ and $n = d/\ell$. With $\alpha_2 = 0.0004$, $\ell = 1$ m and $d = 20$ m: $\sqrt{2 \times 0.0004 \times 8000/3} = 1.46$ m.

Doubling the distance multiplies the sideways standard deviation by $2^{1.5} = 2.83$, and the along-path error, which comes from distance noise alone, by only $\sqrt{2} = 1.41$.

What the first-order propagation misses

The Jacobians treat the heading error as small. But a heading error $\theta$ also shortens the forward progress by the factor $\cos\theta \approx 1 - \theta^2/2$, a second-order effect that the Jacobians ignore (the last module showed the same effect for $\cos$). At 20 m the heading standard deviation is 7° and this adds along-path variance that the first-order calculation does not see.

Because heading error is integrated over distance, the pose covariance of dead reckoning is a long, curved, strongly correlated band. Its sideways spread grows like d^1.5, and a diagonal covariance would hide the correlation between position and heading.

A worked example

A robot drives 20 steps of 1 m straight ahead, with $\alpha_1 = \alpha_2 = \alpha_3 = \alpha_4 = 0.0004$. Each step has a distance noise of $\sqrt{0.0004} = 2$ cm and a noise of 0.02 rad (1.15°) in each of the two turns. The program propagates the covariance with the Jacobians and compares it with 4000 Monte Carlo runs.

1. One step. At the start, $\phi = 0$, so $F = I + \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{pmatrix}$ (with $\delta_{\text{trans}} = 1$) and $G = \begin{pmatrix} 0 & 1 & 0 \\ 1 & 0 & 0 \\ 1 & 0 & 1 \end{pmatrix}$. The control noise is $M = \mathrm{diag}(0.0004, 0.0004, 0.0004)$.
2. After 5, 10 and 20 m. Standard deviations (along, sideways, heading): Monte Carlo (0.045 m, 0.182 m, 3.6°), (0.067, 0.51, 5.1°), (0.127, 1.44, 7.2°); Jacobians (0.045, 0.184, 3.6°), (0.063, 0.518, 5.1°), (0.089, 1.462, 7.3°).
3. The d^1.5 law. Sideways: 0.18, 0.51, 1.44 m at 5, 10, 20 m. From 5 m to 10 m it grows by 2.8, from 10 to 20 m by 2.8 again: the factor $2^{1.5}$. The formula $\sqrt{2\alpha_2 d^3/3}$ gives 1.461 m at 20 m.
4. Where first order fails. The sideways and heading entries agree with Monte Carlo to 2%. The along-path standard deviation at 20 m does not: 0.127 m against 0.089 m. The first-order calculation sees only the distance noise, and misses the loss of forward progress caused by the 7° heading spread (the mean final x is 19.92 m, not 20).
5. The velocity model. A command of $v = 1$ m/s and $\omega = 0.5$ rad/s for 1 s moves the robot along an arc to $x = \sin(0.5)/0.5 = 0.9589$, $y = (1 - \cos 0.5)/0.5 = 0.2448$, heading 0.5 rad: shorter than the 1 m of arc length because the path curves.

import math
from robotprob import (Rng, sample_motion_odometry, motion_odometry, cov_matrix, mean_vector,
                       matmul, transpose, madd, symmetrize, zeros, sample_motion_velocity)

alphas = (0.0004, 0.0004, 0.0004, 0.0004)       # a1: rot from rot, a2: rot from trans, a3: trans from trans, a4: trans from rot

def jacobians(pose, odom):                      # F: d pose' / d pose,  G: d pose' / d control
    rot1, trans, rot2 = odom
    th = pose[2] + rot1
    F = [[1.0, 0.0, -trans * math.sin(th)], [0.0, 1.0, trans * math.cos(th)], [0.0, 0.0, 1.0]]
    G = [[-trans * math.sin(th), math.cos(th), 0.0], [trans * math.cos(th), math.sin(th), 0.0], [1.0, 0.0, 1.0]]
    return F, G

def control_cov(odom, a):                       # the variances of the three movements
    rot1, trans, rot2 = odom
    a1, a2, a3, a4 = a
    return [[a1 * rot1 ** 2 + a2 * trans ** 2, 0.0, 0.0],
            [0.0, a3 * trans ** 2 + a4 * (rot1 ** 2 + rot2 ** 2), 0.0],
            [0.0, 0.0, a1 * rot2 ** 2 + a2 * trans ** 2]]

step = (0.0, 1.0, 0.0)                          # drive 1 m straight, repeated 20 times
pose, P = [0.0, 0.0, 0.0], zeros(3, 3)
cov_track = {}
for k in range(1, 21):                          # propagate the covariance with the Jacobians
    F, G = jacobians(pose, step)
    P = symmetrize(madd(matmul(matmul(F, P), transpose(F)), matmul(matmul(G, control_cov(step, alphas)), transpose(G))))
    pose = motion_odometry(pose, step)
    cov_track[k] = [row[:] for row in P]

rng = Rng(9)                                    # the reference: sample the model many times
runs = 4000
finals = {5: [], 10: [], 20: []}
for _ in range(runs):
    p = [0.0, 0.0, 0.0]
    for k in range(1, 21):
        p = sample_motion_odometry(rng, p, step, alphas)
        if k in finals:
            finals[k].append(p)
print('after n metres of straight driving (std in m, m, degrees):')
for k in (5, 10, 20):
    mc = cov_matrix(finals[k])
    lin = cov_track[k]
    print(k, 'm  Monte Carlo', [round(math.sqrt(mc[0][0]), 3), round(math.sqrt(mc[1][1]), 3), round(math.degrees(math.sqrt(mc[2][2])), 2)],
          ' Jacobians', [round(math.sqrt(lin[0][0]), 3), round(math.sqrt(lin[1][1]), 3), round(math.degrees(math.sqrt(lin[2][2])), 2)])
print('mean final position after 20 m: x', round(mean_vector(finals[20])[0], 3), ' y', round(mean_vector(finals[20])[1], 3))
print('lateral std at 20 m from the formula sqrt(2 a2 d^3 / 3) =', round(math.sqrt(2 * 0.0004 * 20 ** 3 / 3), 3))

x, y, th = sample_motion_velocity(Rng(1), [0.0, 0.0, 0.0], 1.0, 0.5, 1.0, 0.0, 0.0)     # the velocity model without noise
print('arc with v = 1 m/s, w = 0.5 rad/s for 1 s: x', round(x, 4), ' y', round(y, 4), ' theta', round(th, 4))

after n metres of straight driving (std in m, m, degrees):
5 m  Monte Carlo [0.045, 0.182, 3.56]  Jacobians [0.045, 0.184, 3.62]
10 m  Monte Carlo [0.067, 0.51, 5.14]  Jacobians [0.063, 0.518, 5.12]
20 m  Monte Carlo [0.127, 1.437, 7.18]  Jacobians [0.089, 1.462, 7.25]
mean final position after 20 m: x 19.92  y 0.011
lateral std at 20 m from the formula sqrt(2 a2 d^3 / 3) = 1.461
arc with v = 1 m/s, w = 0.5 rad/s for 1 s: x 0.9589  y 0.2448  theta 0.5

In robotics

Localizers use this model every step

A particle filter draws one sample of the model per particle and per step (sample_motion_odometry): the cloud of particles after a move is the banana of the figure. An extended Kalman filter instead propagates the covariance with the Jacobians above (module 8). Navigation stacks expose the four parameters under the names alpha1 to alpha4, with a fifth for omnidirectional bases, in their particle-filter localizer configuration. Wrongly small values make the filter overconfident after a long featureless corridor; wrongly large ones make it jumpy.

Tuning intuition

The parameters can be read off the behaviour. A robot that arrives too far to the side after long straight drives needs a larger $\alpha_2$. One that overshoots or undershoots in distance needs a larger $\alpha_3$. One whose heading is poorly known after turning in place needs a larger $\alpha_1$. M5-T4: Identifying Noise Parameters from Data shows how to estimate them from logged runs.

Systematic errors need calibration, not noise

The model treats odometry errors as random. The largest real errors are often systematic: wheels of slightly different diameters make a straight command curve to one side, a wrong wheel base makes every 90° turn 88°. These errors do not average out and do not look like noise: they look like drift in one direction. Calibration procedures such as driving a square clockwise and counter-clockwise separate the two main systematic errors (Borenstein and Feng's UMBmark); only the remainder is noise for $\alpha$.

The velocity model for planning

When no wheel odometry exists, as for a legged robot or in a planner that predicts motion before it happens, the velocity model describes the arc a commanded $(v, \omega)$ produces. Dynamic window and pure-pursuit planners use these arcs (see the Navigation course). The SLAM course covers the same models for localization in Odometry and Motion Models.

Other robots

For drones the motion model is the IMU integration of the next module and the UAV course; for manipulators it is the joint-space dynamics. The structure is always the same: a deterministic update plus noise whose size depends on what the robot did, and Jacobians for the covariance.

Common misconceptions

Key takeaways

1. The odometry model describes a movement as turn, drive, turn, each with Gaussian noise whose variance grows with the square of the size of the movement (α₁ to α₄).
2. The velocity model moves the robot along a circular arc for a commanded (v, ω), with a straight-line limit as ω → 0; noise is added to v and ω.
3. The Jacobians F (with respect to the old pose) and G (with respect to the control) give the covariance after a step: Σ′ = FΣFᵀ + GMGᵀ.
4. A random heading error integrates into sideways position error: the sideways standard deviation grows like d^1.5, the along-path like √d. The cloud of poses is a curved band.
5. First-order propagation is accurate for the sideways and heading spread but misses second-order loss of forward progress. Systematic errors are calibrated, not modelled as noise.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, sections 5.3 (velocity motion model) and 5.4 (odometry motion model, with table 5.6). Paid book.
• N. Chebrolu and C. Stachniss, “Motion Models” (video lecture) , the odometry and velocity models. Free to watch.
• J. Borenstein and L. Feng, “Measurement and correction of systematic odometry errors in mobile robots”, IEEE Transactions on Robotics and Automation 12 (6), 1996. The UMBmark calibration. Paid journal article.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, Cambridge University Press, 2024 , chapter “Pose Estimation Problems” (motion models for poses). Free to read as the author’s PDF; the book is copyrighted.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

The motion model says where the robot may be after moving. The next lesson builds the other half: given where the robot is, what will its sensors read?

Related content

Nothing below is required to finish this lesson.

• Before
  M5-T1: State, Controls, Measurements and the Markov Assumption

  Draw the dynamic Bayes network of a robot, state the Markov assumption, and say what must be in the state for it to hold.

• Lab
  M5-L1: Probabilistic Odometry

  Dead reckoning puts Rover on the far side of a wall after two corners. Implement the odometry motion model, grow the uncertainty with distance, and identify the wheel noise from logged runs.

Other courses on this site treat neighbouring topics from their own angle:

• SLAM M1-T5: Odometry and Motion Models
• SLAM: Motion and Observation Models
• Robot Foundations 05: Where Am I?
• UAV M3-T7: IMU noise and bias modelling