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

A Careful Control Loop

This section develops the technical contract for Bayesian filtering: Kalman, EKF, particle filters into a usable mental model. First we define the object of study, then we connect it to the agent loop, then we test it with a compact implementation.

The key question in Bayesian filtering: Kalman, EKF, particle filters is practical: 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?

Theory

For Bayesian filtering: Kalman, EKF, particle filters, 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 the initial guess of 0 toward roughly 0.8 to 1.0 within a handful of steps, and the position variance shrinks monotonically as repeated measurements add information. The filter recovers an unmeasured quantity (velocity) purely from the structure of the motion model and the time-correlation of position measurements.

Nonlinear Beliefs: EKF and Particle Filters

Real robots rarely have linear dynamics. A differential-drive robot turning by a heading angle, a range-bearing landmark measurement, 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.

When the belief itself 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 and represents the belief as a cloud of weighted samples (particles). Each particle is a hypothesis about the state; the predict step pushes every particle through the (possibly nonlinear) motion model with sampled noise, the update step reweights each particle by how well it explains the measurement, and a resampling step concentrates particles in high-probability regions. This Monte Carlo approach is the basis of AMCL, the standard ROS localization stack, and it shines exactly where the Kalman family struggles: the kidnapped-robot problem, ambiguous symmetric environments, and heavy-tailed sensor noise. The cost is compute, which grows with the number of particles, and the risk is particle impoverishment, where resampling discards the rare particle that happened to be correct.

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.

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.