Section 8.6: Bayesian filtering: Kalman, EKF, particle filters

"Commit to nothing; carry a distribution. Update when the measurement arrives, not before."

A Prior With Reasonable Uncertainty

A self-driving car merging onto a highway at 30 meters per second receives a GPS fix only once per second. Yet it must place itself within a lane (about 3 meters wide) at every millisecond in between. It survives the 999 milliseconds of GPS silence by carrying a probability distribution over where it might be and propagating that distribution forward through its own motion. That trick, holding a belief instead of a point and reshaping it the instant evidence arrives, is the entire content of Bayesian filtering, and this section builds it from the linear Gaussian case up to the full non-Gaussian particle cloud. Figure 8.6A pictures this probability cloud being reshaped at every sensor reading.

Before that belief-propagating machinery can be built, the same engineering questions that govern any agent module have to be answered for the filter. A filter design answers four practical questions: what must the agent know, what can it observe, what action is available, and what evidence shows that the action worked under the stated conditions. Figure 8.6.B lays out the recurring loop that every filter in this section follows, where a prior belief is pushed through a motion model, corrected by a sensor measurement, and then handed forward as the prior for the next time step.

Theory

The practical design rule is to make the interface inspectable before optimization begins: inputs, outputs, units, latency, bounds, and failure labels should all be visible in the saved artifact.

The Kalman Filter

The Kalman filter is the exact recursive solution to state estimation when the dynamics and the measurement are linear and all noise is Gaussian. It keeps a belief about the state as a mean vector \(\hat{x}\) and a covariance matrix \(P\), and it alternates two steps every time step: a predict step that pushes the belief forward through the motion model, and an update step that pulls it back toward a fresh measurement.

Let the state evolve as \(x_k = A_k x_{k-1} + B_k u_k + w_k\) with process noise \(w_k \sim \mathcal{N}(0, Q_k)\), and let the sensor report \(z_k = H_k x_k + v_k\) with measurement noise \(v_k \sim \mathcal{N}(0, R_k)\). The predict step is

The superscript minus marks the prior belief, before the measurement is seen. Notice that \(P\) always grows in the predict step: pushing the belief through an imperfect model can only add uncertainty. The update step then incorporates the measurement \(z_k\):

The matrix \(K_k\) is the Kalman gain, and it is the heart of the filter. Read it as a trust dial. When \(K_k \to 0\) the filter ignores the measurement and rides on the prediction; when \(K_k \to I\) (in the scalar sense) it throws away the prediction and snaps to the sensor. The gain is computed automatically from the relative sizes of the prior covariance \(P^-_k\) and the measurement noise \(R_k\): a confident prediction or a noisy sensor lowers the gain, and a vague prediction or a sharp sensor raises it.

The term \(z_k - H_k \hat{x}^-_k\) is the innovation, the difference between what the sensor actually reported and what the prediction expected it to report. A filter that is working well produces an innovation that looks like zero-mean white noise; a biased or growing innovation is the clearest signal that the model or the noise settings are wrong.

Worked Example: a 1D Constant-Velocity Tracker

The cleanest way to feel the predict-update loop is to track a target that moves at roughly constant velocity while measuring only its position. The state is \(x = [\text{position}, \text{velocity}]^T\). The motion model says position advances by velocity times \(\Delta t\) and velocity stays put, so \(A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}\). The sensor sees position only, so \(H = [1\ \ 0]\). Velocity is never measured directly; the filter infers it from how position changes, which is exactly what makes the example instructive.

# 1D constant-velocity Kalman filter.
# State x = [position, velocity]^T. We measure noisy position only.
import numpy as np

dt = 1.0
A = np.array([[1.0, dt],      # position += velocity * dt
              [0.0, 1.0]])    # velocity constant
H = np.array([[1.0, 0.0]])    # measure position only
Q = np.array([[0.01, 0.0],    # process noise: the model is not perfect
              [0.0, 0.01]])
R = np.array([[1.0]])         # measurement noise variance (sigma^2 = 1)

# Ground truth: starts at 0, moves at 1.0 unit/step.
rng = np.random.default_rng(7)
steps = 20
true_pos = np.arange(steps) * 1.0
meas = true_pos + rng.normal(0.0, 1.0, size=steps)

x = np.array([[0.0], [0.0]])  # initial state guess
P = np.eye(2) * 5.0           # initial uncertainty (deliberately high)

print(" k  meas   est_pos  est_vel   P_pos")
for k in range(steps):
    # Predict
    x = A @ x
    P = A @ P @ A.T + Q
    # Update
    z = np.array([[meas[k]]])
    S = H @ P @ H.T + R
    Kk = P @ H.T @ np.linalg.inv(S)        # Kalman gain
    x = x + Kk @ (z - H @ x)               # innovation correction
    P = (np.eye(2) - Kk @ H) @ P
    print(f"{k:2d} {meas[k]:6.2f}  {x[0,0]:7.2f}  {x[1,0]:6.2f}  {P[0,0]:6.3f}")

print(f"\nfinal velocity estimate = {x[1,0]:.3f} (truth = 1.000)")
print(f"final position variance = {P[0,0]:.3f} (started at 5.0)")

Running this prints a converging trace. The velocity estimate climbs from 0 toward 1.0 within a handful of steps, and the position variance shrinks monotonically as measurements accumulate. Raw sensor error stays near sigma = 1.0; the filter collapses position variance from 5.0 to about 0.37 over the same 20 steps, a 13x reduction, by squeezing information out of every past sample through the motion model. That same structure recovers velocity, a quantity the sensor never measures, from the time-correlation of position readings. A filter that cannot say how wrong it might be is not a state estimator; it is a rumor.

Nonlinear Beliefs: EKF and Particle Filters

That clean convergence held only because every equation above was linear; the moment the motion or the sensor curves, the exact Kalman machinery no longer applies and must be approximated. Real robots rarely have linear dynamics. A differential-drive robot turning by a heading angle, a range-bearing landmark measurement (a sensor reporting distance and angle to a known landmark, rather than the landmark's x and y coordinates directly), and a camera projection are all nonlinear. The extended Kalman filter (EKF) handles this by linearizing the nonlinear functions around the current estimate. If the motion model is \(x_k = f(x_{k-1}, u_k)\), the EKF replaces \(A_k\) with the Jacobian

evaluated at the current estimate, and similarly replaces \(H_k\) with the Jacobian of the measurement function. The covariance equations then run exactly as before but with these local linear approximations. The EKF is the workhorse of classical robot localization and SLAM, but it inherits one weakness: if the function is strongly curved near the estimate, or if the estimate is far from the truth, the linearization point is poor and the filter can diverge while still reporting a confident covariance.

The unscented Kalman filter (UKF), mentioned earlier and used later in this section's comparison table, sidesteps the Jacobian entirely: instead of linearizing \(f\) and \(h\) analytically, it deterministically picks a small set of sample points around the current estimate, called sigma points, chosen so their weighted mean and covariance exactly match \(\hat{x}_{k-1}\) and \(P_{k-1}\). Each sigma point is pushed through the true nonlinear function \(f\) (no linearization needed), and the resulting spread of transformed points is used to reconstruct the predicted mean \(\hat{x}^-_k\) and covariance \(P^-_k\). The same idea applies to the measurement update. Because the UKF never needs a derivative, it avoids Jacobian errors and tends to track curved functions more accurately than the EKF at a similar computational cost, though it still assumes the belief stays approximately Gaussian, so it cannot replace the particle filter for genuinely multimodal beliefs.

When the belief is non-Gaussian (multimodal, sharply bounded, or shaped by discrete hypotheses), no single mean-and-covariance pair describes it. The particle filter abandons the Gaussian assumption entirely. It represents the belief as a cloud of weighted samples called particles. Each particle is one hypothesis about the state. The predict step pushes every particle through the motion model with sampled noise. The update step reweights each particle by how well it explains the measurement. A resampling step then concentrates particles in high-probability regions. This Monte Carlo approach underlies Adaptive Monte Carlo Localization (AMCL), the standard ROS localization stack. Particles are a democracy of hypotheses, not a dictatorship of the mean: the filter handles the kidnapped-robot problem, symmetric environments, and heavy-tailed noise (measurement errors with occasional large outliers that a Gaussian model badly underestimates) where the Kalman family fails. The cost is compute, which grows with particle count. The risk is particle impoverishment, where resampling discards the rare particle that happened to be correct.

In embodied AI, this matters because physical robots encounter genuinely multimodal uncertainty. A warehouse robot entering a long, symmetric corridor cannot distinguish which end it entered from odometry alone, so a Kalman filter collapses to one wrong location and never recovers. Control actions built on that false certainty cause collisions or mission failures. Turning toward a dock that is actually behind the robot is one such action. Particles keep both hypotheses alive until a distinguishing feature, a sign, a lighting difference, a landmark, resolves the ambiguity. In practice, a ROS AMCL setup with a few hundred particles and a reasonably distinctive environment often recovers correct localization within on the order of ten update cycles, though the exact count depends on particle count, sensor quality, and how distinguishable the two locations are; the equivalent EKF typically does not recover at all within that window, since the underlying linear-Gaussian assumption means new evidence can shift a single Gaussian's mean but cannot split it into two competing hypotheses. The consequence is not abstract: a single Gaussian belief feeding a planner can cause a physical robot to act with confidence in the wrong place.

Inside the particle filter: predict, weight, resample

The mechanism runs in three repeating steps. In the predict step, each particle is propagated forward by sampling the motion model: a particle at pose \((x, y, \theta)\) after command \((v, \omega)\) draws a noise sample and lands at a slightly different pose, so the cloud spreads to reflect motion uncertainty. In the update step, each particle receives a weight proportional to \(p(z \mid x_i)\), the likelihood that the current sensor reading would be observed if the robot were truly at that particle's pose; a particle near a shelf pattern that matches the laser scan gets high weight, one in empty space gets near zero. In the resample step, particles are drawn with replacement from the weighted distribution so high-weight particles multiply and low-weight ones vanish, concentrating the cloud where the evidence is strong.

Practical Recipe

1. Write the observation, action, and success metric before choosing a model.
2. Build a baseline that is simple enough to debug by inspection.
3. Add the library implementation only after the baseline behavior is understood.
4. Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
5. Run at least one perturbation test before trusting the result.

Production Pattern

Bayesian filtering: Kalman, EKF, particle filters sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time, and sensor fusion combines those imperfect signals into a coherent belief.

A Bayesian filter is a predict-update loop whose covariance is part of the output, not a decoration. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

Use this recipe when turning Bayesian filtering: Kalman, EKF, particle filters into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Define each sensor message with units, frame, timestamp source, calibration file, and covariance meaning.
2. Run a static test, a slow-motion test, and a dropout test before fusing streams.
3. Compare the hand filter with FilterPy or ROS 2 robot_localization using identical measurements and noise settings.
4. Log innovation, covariance, delayed messages, rejected measurements, and downstream control effect.
5. Treat perception output as a belief with uncertainty, not as ground truth handed to the controller.

Filtering failures come from a wrong process model, bad measurement likelihood, overconfident covariance, delayed updates, or particle impoverishment. Audit prediction, update, gating, and reset before changing the policy.