19 · Beliefs

Part IV — State Uncertainty

Executed in: Julia · Book: ADM Ch. 19 · Lecture: to be added

Big picture

Where this chapter fits

In Parts II and III, the agent always knew which state it was in: the rover knew its cell, even when it didn’t know where its wheels would take it next. Part IV (Chapters 19–23) drops that assumption. A robot has noisy sensors, a doctor sees symptoms rather than the disease, and a tracker sees radar blips rather than the aircraft. The agent can’t act on the state, because it doesn’t know it. So it acts on a belief: a probability distribution over the states it might be in.

This chapter is about keeping that belief up to date. It is Chapter 3’s inference, repeated at every time step, with Chapter 2’s building blocks as the models. Chapters 20–23 then decide what to do given a belief, treating the belief itself as the state of a new, larger decision problem.

The question: as an agent acts and receives noisy observations, how should it keep track of where it probably is?

The short answer: by repeating two moves forever. Predict: push the belief through what the action does, so it spreads out. Correct: reweight it by how well each possible state explains what you just observed, so it sharpens. The rest of the chapter is about storing the belief so that these two moves stay cheap.

ch19 cluster_store How the belief is stored belief Belief b where might I be? predict Predict  (apply T) belief spreads out belief->predict act correct Correct  (apply O) belief sharpens predict->correct observe table Table of probabilities discrete filter correct->belief b′ samples Cloud of samples particle filter gauss Mean + covariance Kalman filter
Figure 1: Map of this chapter. Every filter runs the same predict–correct loop; they differ in how they store the belief. The book also covers belief initialization, the extended and unscented Kalman filters for nonlinear models, and particle injection (ADM §19.1, §19.4–19.5, §19.7).

The key ideas at a glance

  1. A belief replaces the state. A belief gives each possible state a probability, given everything done and observed so far. It needs one extra piece of model beyond an MDP: an observation model, the probability of each possible observation in each state. For example, the chance the window looks dark when the tram is in a tunnel.

  2. Every update is predict, then correct. Predict: for each possible new state, add up all the ways of getting there from where you might have been. The belief spreads out. Correct: multiply each state’s probability by how likely the actual observation is in that state, then normalize. The belief sharpens. This is Bayes’ rule applied over and over, and it’s called the Bayes filter.

  3. With few states, store the whole table: the discrete filter. Keep one probability per state and apply predict and correct exactly. It’s simple and exact, and it costs about (number of states)² per step.

  4. With linear motion and Gaussian noise, store a mean and a covariance: the Kalman filter. If the belief starts as a bell curve and everything is linear with Gaussian noise, it stays a bell curve. So only its center and spread need updating. The key quantity is the Kalman gain, how far to move from the prediction toward the observation. A noisy sensor gives a small gain; a shaky prediction gives a large one. For nonlinear models, the extended and unscented Kalman filters approximate the same idea.

  5. Otherwise, store samples: the particle filter. Represent the belief by a cloud of guesses (particles). Move each guess with a random draw from the dynamics, weight it by how well it explains the observation, then resample: copy the heavy ones and drop the light ones. The cloud can take any shape, including a ring or two separate clumps. It only needs a simulator of the world, and more particles give more accuracy.

Vocabulary

Table 1: Terms used on this page.
Term Plain meaning
Belief \(b(s)\) Probability that the current state is \(s\), given the history
Observation model \(O(o \mid a, s')\) Probability of seeing \(o\) after action \(a\) lands you in \(s'\)
Predict / correct Push the belief through the dynamics / reweight it by the observation
Bayes filter Predict-then-correct, repeated every step
Kalman gain \(K\) Fraction of the gap between prediction and observation to move by
Innovation The observation minus what you expected to observe
Particle One sampled guess of the state
Resampling Copying high-weight particles and dropping low-weight ones
Particle degeneracy Nearly all weight falling on one or two particles

After this chapter you can

  • Update a belief over a handful of states by hand, and in code for many.
  • Run a Kalman filter, and explain its gain in terms of trust in the prediction versus the sensor.
  • Track a state with a particle filter when the belief has several peaks or odd shapes.
  • Choose between the three representations for a given problem.

Intuition

TipAnalogy

A ship’s navigator before GPS works in two alternating moves.

Dead reckoning (predict). “We sailed north at 10 knots for an hour, so we’re about 10 miles north of where we were.” But currents and wind add error, so every hour of dead reckoning makes the navigator less sure. The circle of possible positions on the chart grows.

A sighting (correct). A lighthouse comes into view at a known bearing. Positions that fit that sighting become more likely, and positions that don’t become less likely. The circle shrinks.

Uncertainty grows when you move and shrinks when you look. A filter is that routine written as math, run at every step.

What the belief looks like depends on the problem. In Figure 2, a tram rider on a 12-stop loop can’t see the stop signs, only whether the window is dark (tunnel) or light. At first every stop is equally likely. As the pattern of tunnels and open track goes by, the belief concentrates on the right stop, and it never becomes perfectly sure, because the tram sometimes lingers or skips a stop.

Figure 2: Discrete filter on the tram. Each column is the belief over the 12 stops after that step’s observation; darker blue is more probable. Diamonds mark the true stop. The top strip shows what the sensor read.

Predict spreads, correct sharpens

The tram starts with every stop equally likely, \(1/12\) each. The window goes dark. Five stops are in tunnels, where “dark” is 0.9 likely, against 0.2 elsewhere. So each tunnel stop’s share becomes \(0.9/5.9 = 0.153\) and each open stop’s \(0.2/5.9 = 0.034\): the belief has sharpened toward the tunnels. Then the tram drives. Each stop’s probability flows mostly one stop forward, with some staying put and some skipping ahead, so the peaks blur and shift. Every step repeats these two moves. See Equation 3.

Kalman: blending two noisy guesses

A kayaker’s predicted position is 2 m, uncertain with variance 10; the GPS says 3.5 m, with variance 4. Neither is trustworthy alone. The Kalman filter trusts each in proportion to the other’s uncertainty. The gain is \(10/(10+4) = 0.71\), so it moves 71% of the way from the prediction toward the GPS, to 3.07 m. The combined variance, 2.86, is smaller than both sources: two independent opinions beat either one. See Equation 6.

Particles: a crowd of guesses

A rover can only measure its distance to one beacon. After the first reading, every point on a ring around the beacon is equally plausible. No bell curve can represent a ring, but a cloud of 2,000 random guesses, reweighted by the reading, forms one on its own. Driving straight east leaves the rover’s side of the beacon ambiguous, so the cloud splits into two mirror-image clumps. After a turn north, the wrong clump’s predictions stop matching the readings, and it dies out. See Equation 7 and Figure 6.

Choosing how to store the belief

A table is exact but grows with the number of states. A mean and covariance are tiny and exact, but only for linear-Gaussian worlds. Particles fit any shape but need many samples when the sensor is precise. The three filters make exactly this trade-off, and the cheat sheet summarizes when to use each.

Formalism

Beliefs

\[ b(s) = P(s_t = s \mid a_{1:t},\ o_{1:t}) \tag{1}\]

In words. \(b(s)\) is how probable it is that the current state is \(s\), given every action taken and every observation received so far.

Term by term.

  • \(s_t\) is the (hidden) state at time \(t\).
  • \(a_{1:t}\) and \(o_{1:t}\) are the whole history of actions and observations.
  • \(b\) is a probability distribution: \(b(s) \ge 0\) and it sums (or integrates) to 1.

Keeping the whole history would grow without bound. The point of a filter is that the belief summarizes the history: to update it, you need only the old belief, the latest action, and the latest observation.

The model now has one more piece than an MDP. Besides \(T(s' \mid s, a)\), there is an observation model \(O(o \mid a, s')\): the probability of observing \(o\) when you took action \(a\) and landed in \(s'\).

The belief update

Updating a belief rests on one famous rule:

\[ P(s \mid o) = \frac{P(o \mid s)\, P(s)}{P(o)} \;\propto\; P(o \mid s)\, P(s) \tag{2}\]

In words. After seeing \(o\), a state’s probability is its old probability times how well it explains \(o\), rescaled so everything sums to 1.

Term by term. \(P(s)\) is the prior, what you believed before looking. \(P(o \mid s)\) is the likelihood, how expected \(o\) is if \(s\) is true. \(P(s \mid o)\) is the posterior, what you believe after looking. \(P(o)\) doesn’t depend on \(s\), so it only rescales. That is why “\(\propto\)” (proportional to) plus normalizing is enough.

Combining Bayes’ rule with the transition model gives the update for one step:

\[ b'(s') \;\propto\; \underbrace{O(o \mid a, s')}_{\text{correct}} \;\underbrace{\sum_{s} T(s' \mid s, a)\, b(s)}_{\text{predict}} \tag{3}\]

In words. First predict where you probably are after acting. Then reweight each candidate by how well it explains what you saw, and normalize.

Term by term.

  • \(\sum_{s} T(s' \mid s, a)\, b(s)\) is the prediction. Add up every way of reaching \(s'\): start in \(s\) (probability \(b(s)\)), then move to \(s'\) (probability \(T(s' \mid s, a)\)). This is the law of total probability, and it spreads the belief out.
  • \(O(o \mid a, s')\) is the correction, the likelihood from Equation 2. It sharpens the belief toward states consistent with \(o\).
  • \(\propto\): divide by the sum over \(s'\) so that \(b'\) sums to 1.

Why it holds. It is Equation 2 with the prediction as the prior. That works because of two Markov assumptions: the next state depends only on the current state and action, and the observation depends only on the action and the state you land in. So the old belief carries everything the past can tell you.

For a finite set of states, Equation 3 is the algorithm: the discrete state filter (ADM §19.2). It costs \(O(|\mathcal{S}|^2)\) per step for the prediction.

Try it. On the tram, start uniform (\(b = 1/12\)) and read “dark”. Tunnel stops explain that with probability 0.9 and open stops with 0.2. Five stops are tunnels, so the normalizer is \(5(0.9) + 7(0.2) = 5.9\). Each tunnel stop gets \(0.9 / 5.9 \approx 0.153\) and each open stop gets \(0.2 / 5.9 \approx 0.034\).

Kalman filter: when everything is Gaussian

Continuous states make the sum an integral, which in general has no closed form. One important case does: linear dynamics with Gaussian noise (ADM §19.3).

\[ T(s' \mid s, a) = \mathcal{N}(s' \mid \mathbf{T}_s s + \mathbf{T}_a a,\ \boldsymbol{\Sigma}_s) \qquad O(o \mid s') = \mathcal{N}(o \mid \mathbf{O}_s s',\ \boldsymbol{\Sigma}_o) \tag{4}\]

In words. The next state is a linear function of the state and action, plus Gaussian noise. The observation is a linear function of the state, plus Gaussian noise.

Term by term.

  • \(\mathcal{N}(x \mid \mu, \Sigma)\) is the Gaussian density at \(x\), with mean \(\mu\) and covariance \(\Sigma\).
  • \(\mathbf{T}_s\) and \(\mathbf{T}_a\) say how the state and the action move the state. For a buoy drifting at constant velocity, \(\mathbf{T}_s\) adds the velocity to the position at each step.
  • \(\boldsymbol{\Sigma}_s\) is the process noise: currents, gusts, anything the model leaves out.
  • \(\mathbf{O}_s\) picks out what the sensor sees, such as position but not velocity. \(\boldsymbol{\Sigma}_o\) is the sensor noise.

If the belief starts Gaussian, \(\mathcal{N}(\boldsymbol{\mu}_b, \boldsymbol{\Sigma}_b)\), it stays Gaussian forever. So the whole belief is just a mean and a covariance, and Equation 3 becomes a few matrix formulas.

\[ \boldsymbol{\mu}_p = \mathbf{T}_s \boldsymbol{\mu}_b + \mathbf{T}_a a \qquad \boldsymbol{\Sigma}_p = \mathbf{T}_s \boldsymbol{\Sigma}_b \mathbf{T}_s^\top + \boldsymbol{\Sigma}_s \tag{5}\]

In words. Move the best guess through the dynamics. The uncertainty is carried along (and stretched or rotated) by the dynamics, then grows by the process noise.

Term by term.

  • \(\boldsymbol{\mu}_p\) is the predicted mean: where the physics says you should be.
  • \(\mathbf{T}_s \boldsymbol{\Sigma}_b \mathbf{T}_s^\top\) is how a linear map transforms a covariance. In 1D with \(\mathbf{T}_s = t\), it is \(t^2 \sigma^2\).
  • \(+\,\boldsymbol{\Sigma}_s\) is the new uncertainty added by the step. This is dead reckoning’s growing circle.

\[ \mathbf{K} = \boldsymbol{\Sigma}_p \mathbf{O}_s^\top \big(\mathbf{O}_s \boldsymbol{\Sigma}_p \mathbf{O}_s^\top + \boldsymbol{\Sigma}_o\big)^{-1} \qquad \boldsymbol{\mu}_b' = \boldsymbol{\mu}_p + \mathbf{K}\big(o - \mathbf{O}_s \boldsymbol{\mu}_p\big) \qquad \boldsymbol{\Sigma}_b' = (\mathbf{I} - \mathbf{K} \mathbf{O}_s)\, \boldsymbol{\Sigma}_p \tag{6}\]

In words. Compare what you observed with what you expected to observe. Move the estimate part of the way toward the observation, by a fraction \(\mathbf{K}\) set by how much you trust the prediction versus the sensor. The uncertainty always shrinks.

Term by term.

  • \(o - \mathbf{O}_s \boldsymbol{\mu}_p\) is the innovation, or surprise: the observation minus the observation you predicted.
  • \(\mathbf{K}\) is the Kalman gain. In 1D with \(\mathbf{O}_s = 1\), it is \(K = \sigma_p^2 / (\sigma_p^2 + \sigma_o^2)\), a number between 0 and 1. A noisy sensor (\(\sigma_o^2\) large) gives \(K \approx 0\): ignore it. A shaky prediction (\(\sigma_p^2\) large) gives \(K \approx 1\): trust the sensor.
  • \((\mathbf{I} - \mathbf{K}\mathbf{O}_s)\boldsymbol{\Sigma}_p\): in 1D, \((1 - K)\sigma_p^2 = \frac{\sigma_p^2 \sigma_o^2}{\sigma_p^2 + \sigma_o^2}\), which is smaller than both \(\sigma_p^2\) and \(\sigma_o^2\).

Why it holds (1D). The corrected belief is proportional to the prior times the likelihood, \(\mathcal{N}(s \mid \mu_p, \sigma_p^2)\, \mathcal{N}(o \mid s, \sigma_o^2)\). A product of two Gaussians in \(s\) is a Gaussian whose precision (inverse variance) is the sum of the two precisions, \(1/\sigma_b^2 = 1/\sigma_p^2 + 1/\sigma_o^2\). Its mean is the precision-weighted average of \(\mu_p\) and \(o\). Rearranging that average gives \(\mu_p + K(o - \mu_p)\) with \(K = \sigma_p^2 / (\sigma_p^2 + \sigma_o^2)\).

When the dynamics or sensor are nonlinear, two variants keep the Gaussian shape by approximation. The extended Kalman filter linearizes the model around the current mean (ADM §19.4). The unscented Kalman filter pushes a few carefully chosen “sigma points” through the true nonlinear model (ADM §19.5).

Particle filter: any shape at all

A Gaussian has one peak. Some beliefs have two (“I’m either north or south of the lighthouse”), and some have odd shapes. A particle filter (ADM §19.6) stores the belief as a cloud of sampled states, or particles, and lets the cloud take any shape:

\[ b(s) \approx \frac{1}{N} \sum_{i=1}^{N} \delta\big(s - s^{(i)}\big) \qquad w^{(i)} \propto O\big(o \mid a, s'^{(i)}\big), \quad s'^{(i)} \sim T\big(\cdot \mid s^{(i)}, a\big) \tag{7}\]

In words. The belief is “wherever the particles are”: a region holding 30% of the particles has probability about 0.3. To update, move every particle with a random draw from the dynamics (predict), score it by how well it explains the observation (the weight \(w\)), then draw a fresh set of \(N\) particles in proportion to those weights (resample).

Term by term.

  • \(\delta(s - s^{(i)})\) is a spike of probability at particle \(s^{(i)}\).
  • \(s'^{(i)} \sim T(\cdot \mid s^{(i)}, a)\) is the predict step, done by simulation. You only need to be able to sample from the model, not to write down its probabilities.
  • \(w^{(i)}\) is the correct step. It is Equation 3’s likelihood, applied to each particle.
  • Resampling copies high-weight particles and drops low-weight ones, so effort goes where the probability is.

The cost is \(O(N)\) per step whatever the shape, but accuracy improves only like \(1/\sqrt{N}\). When the sensor is very precise, almost no particle lands where the likelihood is high. Particle injection adds fresh random particles to recover (ADM §19.7).

Worked example

The tram by hand

The tram is a discrete filter with 12 states and one action (drive). The transition and observation models are two arrays:

import numpy as np

# 12 stops on a loop (0-based here); stops 2, 3, 4, 8 and 10 are in tunnels.
n, tunnels = 12, {2, 3, 4, 8, 10}
# One action (0 = drive): advance 1 stop w.p. 0.8, stay w.p. 0.1, skip ahead 2 w.p. 0.1.
# The % n wraps around the loop: after stop 11 comes stop 0.
T = np.zeros((n, 1, n))
for s in range(n):
    for step, p in ((0, 0.1), (1, 0.8), (2, 0.1)):
        T[s, 0, (s + step) % n] += p
# Light sensor: o = 0 "light", o = 1 "dark". Reads dark 90% of the time in a tunnel, 20% outside.
O = np.zeros((1, n, 2))
for s in range(n):
    p_dark = 0.9 if s in tunnels else 0.2
    O[0, s] = [1 - p_dark, p_dark]
# 12 stops on a loop; stops 3, 4, 5, 9 and 11 are in tunnels.
n, tunnels = 12, [3, 4, 5, 9, 11]
# One action (1 = drive): advance 1 stop w.p. 0.8, stay w.p. 0.1, skip ahead 2 w.p. 0.1.
# mod1 wraps around the loop with 1-based numbering: after stop 12 comes stop 1.
T = zeros(n, 1, n)
for s in 1:n, (step, p) in ((0, 0.1), (1, 0.8), (2, 0.1))
    T[s, 1, mod1(s + step, n)] += p
end
# Light sensor: o = 1 "light", o = 2 "dark". Reads dark 90% of the time in a tunnel, 20% outside.
# O[a, s′, o]: the last axis is the observation, so each O[1, s, :] sums to 1.
O = zeros(1, n, 2)
for s in 1:n
    p_dark = s in tunnels ? 0.9 : 0.2
    O[1, s, :] = [1 - p_dark, p_dark]
end

Step 0: read “dark”. The prior is uniform. As in the “Try it” above, each tunnel stop gets \(0.9/5.9 = 0.153\) and each open stop gets \(0.2/5.9 = 0.034\).

Step 1: predict. The tram drives one step. Take stop 6, which is open and just past the tunnel run 3–5. It can be reached from stop 5 (advance 1, probability 0.8), from stop 6 (stay, 0.1), or from stop 4 (skip 2, 0.1):

\[ b_p(6) = 0.8\,(0.153) + 0.1\,(0.034) + 0.1\,(0.153) = 0.141 \]

So stop 6 is now fairly likely, because the tunnel belief shifted forward by one stop.

Step 1: correct. Suppose the sensor now reads “light”. Every open stop is multiplied by 0.8 and every tunnel stop by 0.1, and then the belief is normalized. Table 2 shows all 12 stops.

Table 2: Hand example: uniform prior, read ‘dark’, drive one stop, read ‘light’. The belief moves to the stops just after a tunnel run.
Stop Tunnel? After ‘dark’ Predicted After ‘light’
1 0.034 0.046 0.075
2 0.034 0.034 0.055
3 yes 0.153 0.046 0.009
4 yes 0.153 0.141 0.029
5 yes 0.153 0.153 0.031
6 0.034 0.141 0.23
7 0.034 0.046 0.075
8 0.034 0.034 0.055
9 yes 0.153 0.046 0.009
10 0.034 0.129 0.21
11 yes 0.153 0.058 0.012
12 0.034 0.129 0.21

Stop 6 comes out on top, and stops 10 and 12 tie for second. All three are open stops right after a tunnel, which is exactly the pattern “dark, then light” suggests. Stop 6 wins because the tunnel before it is three stops long, so more of the “dark” belief flows into it.

A Kalman step by hand

A kayaker paddles downriver about 2 m per step. The position after one step is \(s' = s + 2 + \text{noise}\), with noise variance \(\Sigma_s = 1\). A GPS reads the position with noise variance \(\Sigma_o = 4\). The starting belief is \(\mathcal{N}(0, 9)\). Everything is 1D, so every matrix is a number: \(T_s = T_a = O_s = 1\).

  • Predict (Equation 5): \(\mu_p = 0 + 2 = 2\) and \(\Sigma_p = 9 + 1 = 10\).
  • Gain: \(K = 10 / (10 + 4) = 0.714\). The prediction is shakier than the GPS, so the filter leans toward the GPS.
  • Correct (Equation 6) with a GPS reading \(o = 3.5\). The innovation is \(3.5 - 2 = 1.5\), so \(\mu_b = 2 + 0.714 \times 1.5 = 3.07\) and \(\Sigma_b = (1 - 0.714) \times 10 = 2.86\).

The new variance, 2.86, is smaller than both the prediction’s (10) and the GPS’s (4). Combining two independent sources of information beats either one alone.

# Arrays are 1×1 so the matrix formulas work unchanged.
kayak = dict(Ts=np.eye(1), Ta=np.eye(1), Sigma_s=np.eye(1), Os=np.eye(1), Sigma_o=4 * np.eye(1))
# Fields in order: Ts, Ta, Σs, Os, Σo. In 1D they can be plain numbers.
kayak = LinearGaussianModel(1.0, 1.0, 1.0, 1.0, 4.0)

Interactive

Step through the tram’s filter

Move through time and switch between the belief after predicting and after observing. Watch the predict step blur the belief and shift it forward, and the observation sharpen it again. Gray columns are tunnel stops; the orange triangle is where the tram really is.

Figure 3: The discrete filter on the tram, one step at a time. Bars show the belief over the 12 stops.

How much should you trust the sensor?

This runs the Kalman filter on 25 steps of the kayak, with the GPS readings fixed. You choose the variances the filter assumes. With a small assumed GPS noise \(\Sigma_o\), the estimate chases every reading. With a small assumed process noise \(\Sigma_s\), it trusts its own prediction and smooths the readings away. The data were generated with \(\Sigma_s = 1\) and \(\Sigma_o = 4\).

Figure 4: Kalman filter on the kayak with adjustable assumed noise. The band is the filter’s ±2σ uncertainty.

Algorithms & code

Each tab shows the complete implementation. The Julia code is exactly what produced this page’s figures.

Discrete state filter

This implements Equation 3, split into its predict and correct halves.

# b[s] is the belief; T[s, a, s2] = T(s2 | s, a); O[a, s2, o] = O(o | a, s2).
def predict_discrete(b, T, a):
    return b @ T[:, a, :]    # sum over s of b(s) T(s2 | s, a)

def correct_discrete(b_pred, O, a, o):
    b_new = O[a, :, o] * b_pred
    total = b_new.sum()
    if total == 0:
        raise ValueError(f"observation {o} is impossible under this belief")
    return b_new / total

def update_discrete(b, T, O, a, o):
    return correct_discrete(predict_discrete(b, T, a), O, a, o)
# Discrete state filter (ADM §19.2).
# b[s] is the belief; T[s, a, s′] = T(s′ | s, a); O[a, s′, o] = O(o | a, s′).

# Predict: push the belief through the transition model.
# b_pred(s′) adds up every way of reaching s′: start in s (prob b[s]), then move (prob T[s, a, s′]).
predict_discrete(b, T, a) = [sum(T[s, a, s′] * b[s] for s in eachindex(b)) for s′ in axes(T, 3)]

# Correct: weight each state by how well it explains the observation, then normalize.
function correct_discrete(b_pred, O, a, o)
    b′ = [O[a, s′, o] * b_pred[s′] for s′ in eachindex(b_pred)]
    total = sum(b′)
    total > 0 || throw(ArgumentError("observation $o is impossible under this belief"))
    return b′ ./ total
end

update_discrete(b, T, O, a, o) = correct_discrete(predict_discrete(b, T, a), O, a, o)

Kalman filter

This implements Equation 5 and Equation 6.

def kalman_predict(mu_b, Sigma_b, a, m):
    mu_p = m["Ts"] @ mu_b + m["Ta"] @ a
    Sigma_p = m["Ts"] @ Sigma_b @ m["Ts"].T + m["Sigma_s"]
    return mu_p, Sigma_p

def kalman_correct(mu_p, Sigma_p, o, m):
    Os = m["Os"]
    # (Os Sigma_p Os^T + Sigma_o) is the covariance of the predicted observation.
    # inv() is fine for these small matrices; np.linalg.solve is more stable for big ones.
    K = Sigma_p @ Os.T @ np.linalg.inv(Os @ Sigma_p @ Os.T + m["Sigma_o"])
    mu_b = mu_p + K @ (o - Os @ mu_p)       # move toward the observation by the innovation
    Sigma_b = Sigma_p - K @ Os @ Sigma_p    # the uncertainty always shrinks
    return mu_b, Sigma_b, K

def kalman_update(mu_b, Sigma_b, a, o, m):
    mu_p, Sigma_p = kalman_predict(mu_b, Sigma_b, a, m)
    mu_b, Sigma_b, _ = kalman_correct(mu_p, Sigma_p, o, m)
    return mu_b, Sigma_b
# Kalman filter (ADM §19.3) for a linear-Gaussian model:
#   T(s′ | s, a) = N(s′ | Ts s + Ta a, Σs)     O(o | s′) = N(o | Os s′, Σo)
# The belief is a Gaussian N(μb, Σb). Works with matrices, or with plain numbers in 1D.
struct LinearGaussianModel
    Ts; Ta; Σs; Os; Σo
end

# Predict: move the mean through the dynamics; the covariance grows by Σs.
function kalman_predict(μb, Σb, a, m::LinearGaussianModel)
    μp = m.Ts * μb + m.Ta * a
    Σp = m.Ts * Σb * m.Ts' + m.Σs
    return μp, Σp
end

# Correct: blend prediction and observation using the Kalman gain K.
function kalman_correct(μp, Σp, o, m::LinearGaussianModel)
    # A / B means A * inv(B), computed without forming the inverse.
    # (Os Σp Os' + Σo) is the covariance of the predicted observation.
    K = Σp * m.Os' / (m.Os * Σp * m.Os' + m.Σo)
    μb = μp + K * (o - m.Os * μp)     # move toward the observation by the innovation
    Σb = Σp - K * m.Os * Σp           # same as (I - K Os) Σp, but also works with plain numbers
    return μb, Σb, K
end

function kalman_update(μb, Σb, a, o, m::LinearGaussianModel)
    μp, Σp = kalman_predict(μb, Σb, a, m)
    μb′, Σb′, _ = kalman_correct(μp, Σp, o, m)
    return μb′, Σb′
end

Particle filter

This implements Equation 7. The model enters only through two functions you supply: one that samples a next state, and one that scores an observation.

# sample_next(rng, s, a) draws s2 ~ T(. | s, a); likelihood(o, a, s2) returns O(o | a, s2).
def update_particles(particles, a, o, sample_next, likelihood, rng):
    moved = np.array([sample_next(rng, s, a) for s in particles])   # predict
    w = np.array([likelihood(o, a, s2) for s2 in moved])            # weight
    if w.sum() == 0:
        raise ValueError("every particle has zero weight")
    # Resample: draw N indices with probability proportional to weight. Heavy
    # particles get copied, light ones tend to vanish, and all weights reset to equal.
    keep = rng.choice(len(moved), size=len(moved), p=w / w.sum())
    return moved[keep]
# Particle filter (ADM §19.6). The belief is a vector of sampled states.
#   sample_next(rng, s, a) draws s′ ~ T(· | s, a)
#   likelihood(o, a, s′)   returns O(o | a, s′)

function update_particles(particles, a, o, sample_next, likelihood, rng)
    moved = [sample_next(rng, s, a) for s in particles]    # predict
    w = [likelihood(o, a, s′) for s′ in moved]             # weight
    sum(w) > 0 || error("every particle has zero weight: the observation is impossible under this belief")
    # Resample: draw N particle indices with probability proportional to weight.
    # Heavy particles get copied, light ones tend to vanish; all weights reset to equal.
    keep = rand(rng, Categorical(w ./ sum(w)), length(moved))
    return moved[keep]
end

Run them on the worked examples

b0 = correct_discrete(np.full(n, 1 / n), O, 0, 1)             # uniform prior, read "dark"
b1 = update_discrete(b0, T, O, 0, 0)                           # drive, read "light"
mu_p, Sigma_p = kalman_predict(np.zeros(1), 9 * np.eye(1), np.full(1, 2.0), kayak)
mu_b, Sigma_b, K = kalman_correct(mu_p, Sigma_p, np.full(1, 3.5), kayak)
assert np.isclose(b0[2], 0.9 / 5.9) and b1.argmax() == 5       # stop 6 in 1-based numbering
assert np.allclose([K[0, 0], mu_b[0], Sigma_b[0, 0]], [10 / 14, 2 + 15 / 14, 20 / 7])
b0 = correct_discrete(fill(1 / n, n), O, 1, 2)                 # uniform prior, read "dark"
b1 = update_discrete(b0, T, O, 1, 1)                           # drive, read "light"
μp, Σp = kalman_predict(0.0, 9.0, 2.0, kayak)
μb, Σb, K = kalman_correct(μp, Σp, 3.5, kayak)
Tram: after 'dark' then 'light', the most likely stop is 6 (b = 0.23).
Kayak: K = 0.714, posterior mean = 3.071, variance = 2.857.
  • mod1(s + step, n) wraps around a loop with 1-based numbering: it returns a value in 1:n, where mod would return 0:n-1.
  • struct LinearGaussianModel is like a Python dataclass. Fields without type annotations accept anything, which is why the same code works with numbers in 1D and with matrices in 2D.
  • Σp * m.Os' / (…): ' is the transpose, and A / B means \(A B^{-1}\), computed without forming the inverse.
  • rand(rng, Categorical(w), N) draws \(N\) indices with probabilities w, like rng.choice(len(w), N, p=w) in NumPy.
  • [f(x) for x in xs] comprehensions read just like Python’s. for s in 1:n, (step, p) in pairs is a nested loop on one line.

Results

Kalman filter: tracking a drifting buoy

A buoy drifts on the sea with a roughly constant velocity. Its state is position and velocity, \(s = (x, y, \dot x, \dot y)\), and GPS reports only the position, with 2 m of noise in each direction. The filter never measures velocity directly, yet it learns it from how the positions change. As the velocity estimate improves, each prediction gets better and the uncertainty shrinks (Figure 5). By step 30 the position’s standard deviation is 0.9 m, less than half the GPS noise.

Figure 5: Kalman filter tracking a buoy from noisy GPS fixes. Shaded ellipses are 95% regions of the position belief, drawn every third step; they start large and shrink as the filter learns the buoy’s velocity.

Particle filter: a range-only sensor

A rover can measure only its distance to a single beacon, with 0.5 m of noise, and it knows how it drove. One distance reading puts the rover anywhere on a ring around the beacon. No Gaussian can represent a ring, but a particle cloud can (Figure 6).

While the rover drives straight east, the readings can’t tell north of the beacon from south: both would produce the same sequence of distances. So the cloud splits into two mirror-image clusters. At step 7 the rover turns north. The mirror-image cluster now predicts distances that don’t match, so it dies out.

Figure 6: Particle filter with a range-only sensor. The star is the beacon, gray dots are particles, and the orange diamond is the true position. After one reading the belief is a ring. Driving east leaves two mirror-image clusters; the turn north at step 7 removes the wrong one.

Solved problems

Problem 1: a discrete update by hand. A cat is in one of three rooms, A, B or C. Your belief is \((0.5, 0.3, 0.2)\). Each minute the cat stays put with probability 0.6, or moves to the next room in the cycle A → B → C → A with probability 0.4. You then hear a meow, which you would hear with probability 0.9 if the cat is in A, 0.5 if in B, and 0.1 if in C. What is your new belief?

Predict. Each room can be reached by staying, or by moving from the previous room in the cycle:

  • \(b_p(A) = 0.6(0.5) + 0.4(0.2) = 0.38\) (stay in A, or move C → A)
  • \(b_p(B) = 0.6(0.3) + 0.4(0.5) = 0.38\)
  • \(b_p(C) = 0.6(0.2) + 0.4(0.3) = 0.24\)

Correct. Multiply by the meow likelihoods: \(0.9(0.38) = 0.342\), \(0.5(0.38) = 0.190\), \(0.1(0.24) = 0.024\). These sum to \(0.556\), so

\[ b' = (0.615,\ 0.342,\ 0.043). \]

The prediction made A and B equally likely. The meow, much likelier near A, broke the tie.

Problem 2: the Kalman gain at the extremes. In 1D with \(O_s = 1\), the prediction is \(\mathcal{N}(10, 4)\) and the sensor variance is \(\Sigma_o = 1\). You observe \(o = 12\). Find \(K\), the new mean and the new variance. Then describe what happens as \(\Sigma_o \to 0\) and as \(\Sigma_o \to \infty\).

\(K = 4 / (4 + 1) = 0.8\), so the mean is \(10 + 0.8(12 - 10) = 11.6\) and the variance is \((1 - 0.8) \times 4 = 0.8\).

  • \(\Sigma_o \to 0\) (perfect sensor): \(K \to 1\), the mean jumps to \(o\), and the variance goes to 0. You believe the sensor completely.
  • \(\Sigma_o \to \infty\) (useless sensor): \(K \to 0\), and the mean and variance stay at the prediction. The observation is ignored.

The filter is always a compromise between these two extremes, and \(K\) says where on that line it sits.

Problem 3: steady state. Keep running the kayak’s filter from the worked example, with \(\Sigma_s = 1\) and \(\Sigma_o = 4\). The posterior variance settles to a fixed value \(\Sigma_\infty\). Find it.

At steady state, one predict-and-correct cycle returns the same variance. Predicting gives \(\Sigma_\infty + 1\). Correcting gives

\[ \Sigma_\infty = \frac{(\Sigma_\infty + 1) \cdot 4}{(\Sigma_\infty + 1) + 4} \;\Longrightarrow\; \Sigma_\infty^2 + \Sigma_\infty - 4 = 0 \;\Longrightarrow\; \Sigma_\infty = \frac{\sqrt{17} - 1}{2} \approx 1.56. \]

The steady-state gain is \(K_\infty = 2.56 / 6.56 \approx 0.39\), the value the kayak slider settles to at the true noise levels. The variance doesn’t depend on the observations at all, only on the noise levels. That’s a special property of linear-Gaussian models: you can compute a Kalman filter’s accuracy before collecting any data.

Problem 4: when particles fail. A robot on a 20 m track uses 500 particles spread evenly over the track. Its position sensor is very precise, with noise standard deviation 1 cm. About how many particles land within ±3 cm (three standard deviations) of the true position? What goes wrong, and what are two fixes?

The ±3 cm window covers \(0.06 / 20 = 0.3\%\) of the track, so about \(500 \times 0.003 = 1.5\) particles land in it. Nearly all the weight goes to one or two particles, and resampling fills the cloud with copies of them. This is particle degeneracy: the cloud has collapsed, so it can no longer represent uncertainty, and a single bad draw loses track of the robot.

Some fixes:

  • Use many more particles, which works but is costly.
  • Inflate the likelihood’s variance when computing weights. This throws away some precision to keep the cloud diverse.
  • Add random new particles each step (particle injection, ADM §19.7), so the filter can recover when it loses track.

Recap

  1. When the state is hidden, the agent acts on a belief: a probability distribution over states (Equation 1). Why it matters: this turns a problem with a hidden state into one with a fully known “state” (the belief), which is the key move for all of Part IV.
  2. Every filter repeats predict (Equation 3‘s sum, where uncertainty grows) then correct (Bayes’ rule, where uncertainty shrinks). Why it matters: the same two moves appear in robot localization, object tracking, speech recognition and medical monitoring. Recognizing them makes the variants easy to learn.
  3. The discrete filter stores the full table. It is exact, costs \(O(|\mathcal{S}|^2)\) per step, and suits small state spaces. Why it matters: it is the reference answer that the approximations try to match, and the belief update used by the exact planners in Ch 20.
  4. The Kalman filter is exact for linear-Gaussian models, where the belief is just a mean and covariance (Equation 5, Equation 6). The gain \(K\) trades off trust in the prediction against trust in the sensor. Why it matters: it is cheap and optimal, which is why it runs inside nearly every navigation system. Its variants (EKF, UKF) stretch it to mild nonlinearity.
  5. The particle filter represents any belief shape with samples (Equation 7), including rings and multiple peaks. It needs only a simulator, not a formula, for the dynamics. Why it matters: it is the workhorse when models are nonlinear or multimodal, and it is how online POMDP planners represent beliefs (Ch 22).

Cheat sheet & connections

Table 3: Key equations from this page (★ = well-known by name). \(d\) is the dimension of the continuous state; \(N\) is the number of particles.
Name Formula Ref Cost per step
Belief \(b(s) = P(s_t = s \mid a_{1:t}, o_{1:t})\) Equation 1 —
★ Bayes’ rule \(P(s \mid o) \propto P(o \mid s)\, P(s)\) Equation 2 —
★ Belief update (Bayes filter) \(b'(s') \propto O(o \mid a, s') \sum_s T(s' \mid s, a)\, b(s)\) Equation 3 \(O(\lvert\mathcal{S}\rvert^2)\)
★ Kalman predict \(\boldsymbol\mu_p = \mathbf{T}_s \boldsymbol\mu_b + \mathbf{T}_a a,\ \ \boldsymbol\Sigma_p = \mathbf{T}_s \boldsymbol\Sigma_b \mathbf{T}_s^\top + \boldsymbol\Sigma_s\) Equation 5 \(O(d^3)\)
★ Kalman gain and correct \(\mathbf{K} = \boldsymbol\Sigma_p \mathbf{O}_s^\top (\mathbf{O}_s \boldsymbol\Sigma_p \mathbf{O}_s^\top + \boldsymbol\Sigma_o)^{-1}\), \(\ \boldsymbol\mu_b' = \boldsymbol\mu_p + \mathbf{K}(o - \mathbf{O}_s \boldsymbol\mu_p)\) Equation 6 \(O(d^3)\)
Particle weights \(w^{(i)} \propto O(o \mid a, s'^{(i)})\), \(s'^{(i)} \sim T(\cdot \mid s^{(i)}, a)\) Equation 7 \(O(N)\)

Which filter? Use the discrete filter for small, finite state spaces. Use the Kalman filter when the dynamics and sensor are (close to) linear with Gaussian noise. Use the particle filter when the belief can have several peaks or odd shapes, or when you can simulate the model but not write it down.

Common pitfalls

  • Forgetting to normalize after the correct step. The unnormalized numbers look plausible but aren’t probabilities.
  • An observation the model says is impossible. The normalizer becomes 0 and the belief is undefined. The code on this page raises an error instead of silently returning NaNs. Real systems avoid it with a small floor on every likelihood.
  • Assuming the wrong noise levels. Too little assumed noise makes a Kalman filter overconfident and slow to react. Try it in the kayak slider by making \(\Sigma_s\) tiny.
  • Too few particles for a sharp sensor. See Problem 4.

Connections