M5-T3 — Range, Bearing and Beam Observation Models

Learning objectives

After this lesson, you should be able to:

• Write the range-bearing observation model of a landmark, with wrapped bearing error, evaluate its likelihood, and derive its Jacobian.
• Write the beam model as a mixture of a hit, a short reading, a miss and a random reading, and say what physical cause each part stands for.
• Use a likelihood to weight pose hypotheses and compute a posterior from it.
• Explain why a Gaussian-only model fails when readings can be wrong, and why the product over many beams needs care.

The intuition

A sensor reading depends on where the robot is. Given a pose and a map, the geometry tells what a perfect sensor would read: the expected measurement. A real sensor adds noise. The observation model is the distribution of the reading around that expected value, and the likelihood of a pose is that distribution evaluated at what was actually read.

Landmarks: range and bearing

Rover sees a landmark at a known position, and its sensor reports the distance (range) and the angle from its heading (bearing). From the right-hand picture, three candidate poses each predict a different pair: A (the truth) expects range 3.72 m and bearing 0.33 rad; B, which is 30 cm to one side, expects 3.48 m and 0.38 rad; C, which has the correct position but a heading that is 0.15 rad off, expects the same range as A but bearing 0.18 rad. A measurement of (3.70 m, 0.35 rad) with noise of 0.1 m and 0.05 rad is very close to A's prediction, a bit far from B's, and far from C's in bearing: the likelihoods are 29.4, 2.4 and 0.12, and with equal priors the posterior is 92%, 7.5% and 0.4%.

Note which quantity separates which pose. Range distinguishes B from A (position error), bearing distinguishes C from A (heading error). A range-only sensor could not tell A from C at all, which is why a single landmark seen only by range cannot fix a heading.

Range finders: the beam model

A lidar beam measures the distance to the first thing it hits. That is usually the wall in the map, but not always. The left-hand panel shows the density of a reading, when the map says 3 m, as a mixture of four causes, each with its own shape.

The Gaussian alone would be a fair model of the first row. But a localizer that uses only that is brittle: a single beam blocked by a person gets a likelihood near zero, and that one beam can dominate a product of hundreds. The other three components give such readings a modest, non-zero probability, so one wrong beam costs a little instead of everything.

A good observation model says not only how the sensor behaves when all is well, but also how it fails. The failure components cost almost nothing when the data are clean, and save the localizer when they are not.

Definitions

Expected measurement $h(x, m)$

What a noise-free sensor would read at the pose $x$ in a world $m$. Computed from geometry.

Observation model $p(z \mid x, m)$

The distribution of the reading around the expected measurement. Evaluated at the actual reading it is the likelihood of the pose.

Landmark

A feature with a known position $(l_x, l_y)$ in the world, identified by the sensor.

Beam model

The mixture model of a single range beam, with parameters $\sigma_{\text{hit}}, \lambda_{\text{short}}$ and four weights that sum to 1.

Derivation: the Jacobian, and the truncation constants

The Jacobian of range and bearing

Let $\Delta x = l_x - x$, $\Delta y = l_y - y$ and $q = \Delta x^2 + \Delta y^2$, so $r = \sqrt{q}$.

1. $$\dfrac{\partial r}{\partial x} = \dfrac{1}{2\sqrt{q}}\cdot 2\Delta x\cdot(-1) = -\dfrac{\Delta x}{\sqrt{q}}, \qquad \dfrac{\partial r}{\partial y} = -\dfrac{\Delta y}{\sqrt{q}}, \qquad \dfrac{\partial r}{\partial \theta} = 0$$

   Differentiate the range. Moving the robot towards the landmark shortens the range, and turning on the spot does not change it.

2. $$\dfrac{d}{du}\operatorname{atan2}(a, b) = \dfrac{b\,\dot a - a\,\dot b}{a^2 + b^2}$$

   The derivative of the angle of the vector $(b, a)$ (here $a = \Delta y$, $b = \Delta x$).

3. $$\dfrac{\partial \varphi}{\partial x} = \dfrac{\Delta y}{q}, \qquad \dfrac{\partial \varphi}{\partial y} = -\dfrac{\Delta x}{q}, \qquad \dfrac{\partial \varphi}{\partial \theta} = -1$$

   Since $\partial\Delta x/\partial x = -1$ and $\partial \Delta y/\partial y = -1$. The last entry is the subtraction of the heading. The bearing is the more sensitive to position when the landmark is close (small $q$).

Why the components are renormalized

A probability density must integrate to 1. The hit component is a Gaussian that would extend below zero and above $z_{\max}$, and the short component is an exponential that is defined only up to $z^{*}$. Each is cut off and rescaled so that it integrates to 1 over its range.

1. $$\eta_{\text{hit}} = \Bigl[\Phi\!\bigl(\tfrac{z_{\max} - z^{*}}{\sigma}\bigr) - \Phi\!\bigl(\tfrac{-z^{*}}{\sigma}\bigr)\Bigr]^{-1}$$

   The inverse of the probability mass of the Gaussian that lies inside the sensor range. For $z^{*} = 3$, $\sigma = 0.1$ it is 1; near a wall ($z^{*} < 3\sigma$) it rises towards 2.

2. $$\int_0^{z^{*}} \lambda e^{-\lambda z}\,dz = 1 - e^{-\lambda z^{*}} \;\;\Rightarrow\;\; \eta_{\text{short}} = \dfrac{1}{1 - e^{-\lambda z^{*}}}$$

   The exponential, integrated over the part of the range in front of the expected surface. For $\lambda = 0.5$ and $z^{*} = 3$ it is 1.287: leaving it out would lose 29% of this component.

3. $$w_{\text{hit}} + w_{\text{short}} + w_{\text{max}} + w_{\text{rand}} = 1$$

   The four parts are mixed with weights that sum to 1 (as in the sensor mixture of the last module), so the whole is again a distribution. The weights are found from data (the last lesson of this module).

The product over a scan

With $K$ beams the log-likelihood is a sum of $K$ terms. If each beam were independent evidence, a scan of 360 beams would make the likelihood peak sharply enough that tiny pose errors would be heavily penalized and the filter would collapse onto a few particles. Practical localizers therefore use only a subsample of beams (say 30 spread across the scan), or raise $\sigma_{\text{hit}}$ above the sensor's true noise, or raise the likelihood to a power below 1: all ways of admitting that the beams are not independent.

The likelihood of a pose is the product, over measurements, of the model density at the actual readings. Giving the model a failure mode keeps one bad reading from vetoing a good pose.

A worked example

Two calculations, one program.

1. Range-bearing. Landmark at (4, 3). Measurement $z = (3.70\text{ m},\, 0.35\text{ rad})$, noise $\sigma_r = 0.1$ m, $\sigma_\varphi = 0.05$ rad. Hypothesis A, the true pose (1.0, 0.8, 0.3): the expected measurement is (3.72, 0.333), residuals (−0.02, 0.017), that is 0.2σ and 0.3σ: likelihood $29.4$. B (1.3, 0.8, 0.3): expected (3.483, 0.384), residuals 2.2σ and −0.7σ: likelihood 2.40. C (1.0, 0.8, 0.45): expected (3.72, 0.183), bearing residual 3.3σ: likelihood 0.116.
2. The posterior. With equal priors the normalized likelihoods are 0.921, 0.075 and 0.004. A single landmark measurement has pinned down both the position (via range) and the heading (via bearing).
3. The beam model, four readings. Expected range 3.0 m, $\sigma_{\text{hit}} = 0.1$, $\lambda = 0.5$, weights (0.7, 0.15, 0.05, 0.1), maximum range 8 m. A reading of 3.02 m (a hit) has density 2.75 per m. A reading of 1.5 m (a person in front) has density 0.058: 47 times lower, but far from zero. No echo (8.0 m) gets 0.0625 (the point mass plus the uniform floor). A random reading of 5.5 m gets 0.0125.
4. A whole scan with one blocked beam. Ten beams, nine of them 3.0 ± 0.1 m and the fifth (a person) at 1.5 m. A wrong pose is compared with the true one: it is 0.3 m too far from the wall in nine beams, but its fifth beam happens to expect 1.6 m (a shelf corner, coincidentally where the person stands). Gaussian-only log-likelihoods: true pose −104.2, wrong pose −33.6: the wrong pose wins by 70 nats, because one blocked beam wrecks the true pose. Mixture model: true pose +1.1, wrong pose −19.9: the true pose wins by 21 nats.

import math
from robotprob import Rng, range_bearing, range_bearing_likelihood, bayes_update, beam_likelihood, gaussian_pdf

lm = [4.0, 3.0]                                  # a landmark with a known position
z = [3.70, 0.35]                                 # measured range (m) and bearing (rad)
poses = {'A (true)': [1.0, 0.8, 0.3], 'B (30 cm off)': [1.3, 0.8, 0.3], 'C (0.15 rad off)': [1.0, 0.8, 0.45]}
liks = []
for name, p in poses.items():
    r, phi = range_bearing(p, lm)                # the expected measurement from this pose
    like = range_bearing_likelihood(z, p, lm, 0.1, 0.05)
    liks.append(like)
    print(name.ljust(17), ': expected range', round(r, 3), ' bearing', round(phi, 3), ' likelihood', round(like, 3))
post = bayes_update([1 / 3, 1 / 3, 1 / 3], liks)
print('posterior with equal priors:', [round(v, 3) for v in post])

params = (0.1, 0.5, 0.7, 0.15, 0.05, 0.1)       # sigma_hit, lambda_short, w_hit, w_short, w_max, w_rand
for z_read, what in ((3.02, 'a hit'), (1.5, 'a person in front'), (8.0, 'no echo'), (5.5, 'a random value')):
    print(what.ljust(18), 'z =', z_read, ' p =', round(beam_likelihood(z_read, 3.0, 8.0, *params), 4))

rng = Rng(3)                                     # a scan of ten beams, the fifth blocked by a person
readings = [3.0 + rng.gauss(0.0, 0.1) for _ in range(10)]
readings[4] = 1.5
true_pose = [3.0] * 10                           # expected range of each beam from the true pose
wrong_pose = [3.3] * 10                          # a wrong pose ...
wrong_pose[4] = 1.6                              # ... that happens to expect a surface where the person stands
def loglik(expected, model):
    return sum(math.log(model(zz, e)) for zz, e in zip(readings, expected))
gauss_only = lambda zz, e: max(gaussian_pdf(zz, e, 0.1), 1e-300)
mixture = lambda zz, e: beam_likelihood(zz, e, 8.0, *params)
for name, exp in (('true pose', true_pose), ('wrong pose', wrong_pose)):
    print(name.ljust(10), 'log-likelihood: Gaussian only', round(loglik(exp, gauss_only), 1), ' mixture', round(loglik(exp, mixture), 1))

A (true)          : expected range 3.72  bearing 0.333  likelihood 29.385
B (30 cm off)     : expected range 3.483  bearing 0.384  likelihood 2.398
C (0.15 rad off)  : expected range 3.72  bearing 0.183  likelihood 0.116
posterior with equal priors: [0.921, 0.075, 0.004]
a hit              z = 3.02  p = 2.7498
a person in front  z = 1.5  p = 0.0581
no echo            z = 8.0  p = 0.0625
a random value     z = 5.5  p = 0.0125
true pose  log-likelihood: Gaussian only -104.2  mixture 1.1
wrong pose log-likelihood: Gaussian only -33.6  mixture -19.9

In robotics

Landmark and marker localization

Wherever a robot sees identified features at known positions, such as fiducial markers, reflectors, or a charging dock, the range-bearing model scores poses. The extended Kalman filter of module 8 linearizes it with the Jacobian above; a particle filter evaluates the likelihood directly for every particle. M8-T2: EKF Localization with Landmarks builds the EKF localizer on this lesson. The SLAM course uses the same model for landmark SLAM in Landmark-Based Mapping.

Scan-based localization

Particle filters on a laser scan (Monte Carlo localization, module 9) multiply the beam model over a subsample of beams for each particle. Two parameters of the beam model matter most in practice: the weight of the short component, which sets how crowded an environment the localizer tolerates, and $\sigma_{\text{hit}}$, which sets how precisely it localizes. The alternative likelihood-field model replaces the ray casting by a distance to the nearest obstacle, and costs far less per particle; it is smoother but ignores the physics of the beam.

Sensors without a mixture are fragile

A camera-based detector that returns a position and a covariance, with no allowance for a misdetection, is exposed to the same failure as the Gaussian-only scan. The robust remedy is the mixture of the last module, or gating (the Mahalanobis test of M4-T3: The Multivariate Gaussian and Covariance Ellipses), or a heavy-tailed loss in the optimization (M11-T4: Nonlinear Least Squares and Pose Graph Optimization).

Errors in the model, not in the sensor

The expected measurement comes from the map. A map that is wrong (a moved shelf) produces residuals that look like short or random readings. The mixture absorbs them, which is a feature for transient objects and a problem for permanent changes: a persistent run of “short” readings in the same direction should be treated as evidence that the map has changed (the persistence rule of the sensor-noise lesson).

Common misconceptions

Key takeaways

1. The observation model gives the distribution of a reading around the value that geometry predicts for a pose. At the actual reading it is the likelihood of that pose.
2. For a landmark, range separates position errors and bearing separates heading errors. Bearing differences must be wrapped to (−π, π]. The Jacobian of (range, bearing) has −Δx/√q, −Δy/√q, Δy/q, −Δx/q and −1.
3. The beam model mixes a Gaussian hit, an exponential short reading (unexpected objects), a point mass at maximum range (missed echoes) and a uniform random component, all renormalized.
4. The failure components make one blocked beam cost a little instead of everything; without them a wrong pose can beat the true pose.
5. The product of beam likelihoods assumes independence that does not hold. Subsample, soften or inflate to avoid overconfidence.

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, chapter 6 (robot perception: section 6.3 beam models of range finders, section 6.4 likelihood fields, feature-based measurement models). Paid book.
• C. Stachniss, “Observation Models” (video lecture) , range-bearing and beam models. Free to watch.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, Cambridge University Press, 2024 , chapter “Pose-and-Point Estimation Problems” (observation models for points). Free to read as the author’s PDF; the book is copyrighted.
• C. Stachniss, “Particle Filter and Monte Carlo Localization” (video lecture) , which uses these models to weight particles. Free to watch.

Knowledge check

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

What’s next

We now have both conditional distributions, and both depend on noise parameters that were simply assumed. The last lesson of this module shows how to measure them from data.

Related content

Nothing below is required to finish this lesson.

• Before
  M5-T2: Odometry and Velocity Motion Models

  Write the odometry motion model with its noise parameters, sample from it, and explain the banana-shaped spread of a dead-reckoned pose.

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