Rao-Blackwellization

"I was told to sample a fifteen-dimensional state with a thousand particles. Instead I sampled the three dimensions that were genuinely hard and solved the other twelve exactly. The same thousand particles suddenly did the work of a million."

A Frugal AI Agent

The theorem behind the trick

What Rao-Blackwellization is, at root, is a variance-reduction identity from statistics. The Rao-Blackwell theorem says that if you have any estimator \(\hat{g}\) of a quantity, and \(T\) is a sufficient statistic, then conditioning on \(T\) can only help: the conditional estimator \(\mathbb{E}[\hat{g}\mid T]\) has variance no larger than \(\hat{g}\) itself. In symbols,

with equality only when \(\hat{g}\) was already a function of \(T\). The gap comes from the law of total variance: every bit of variance you can explain away by conditioning is variance you no longer pay for in your estimate. Why this matters for filtering is that Monte-Carlo sampling is a high-variance estimator, and any component of the state we can handle analytically plays the role of "conditioning it away." Instead of letting particles thrash around a dimension that has a closed-form answer, we collapse that dimension into an exact conditional density carried alongside each particle.

Splitting the state: sample this, solve that

How you apply it is to partition the state \(\mathbf{x}_k\) into two blocks:

where \(\mathbf{n}_k\) is the nonlinear (or discrete, or otherwise intractable) part and \(\mathbf{l}_k\) is a part that is conditionally linear-Gaussian: given the whole trajectory \(\mathbf{n}_{0:k}\) of the hard part, the dynamics and measurements of \(\mathbf{l}_k\) are linear with Gaussian noise. The joint posterior then factors exactly:

The first factor is approximated by particles over the hard trajectory, exactly as in Section 10.3. The second factor is a Gaussian, \(\mathcal{N}(\boldsymbol{\mu}_k^{(i)}, \boldsymbol{\Sigma}_k^{(i)})\), one per particle \(i\), maintained in closed form by a Kalman filter conditioned on that particle's sampled history. Each particle therefore carries not just a point \(\mathbf{n}_k^{(i)}\) but a small mean vector and covariance matrix for the linear part. This construction is the Rao-Blackwellized particle filter (RBPF), also called the marginalized particle filter.

The RBPF recursion, one step

Each cycle interleaves a sampling step for the hard part with an exact Kalman step for the easy part. For particle \(i\) at time \(k\):

1. Propagate the hard part. Draw \(\mathbf{n}_k^{(i)} \sim q(\mathbf{n}_k \mid \mathbf{n}_{k-1}^{(i)}, \mathbf{z}_k)\) from a proposal, as usual.
2. Kalman-update the easy part. Using the sampled \(\mathbf{n}_k^{(i)}\) to fix the otherwise-linear model matrices, run one predict/update of a Kalman filter to move \((\boldsymbol{\mu}_{k-1}^{(i)}, \boldsymbol{\Sigma}_{k-1}^{(i)})\) to \((\boldsymbol{\mu}_k^{(i)}, \boldsymbol{\Sigma}_k^{(i)})\).
3. Weight by the marginal likelihood. Multiply the weight by the Gaussian innovation likelihood \(p(\mathbf{z}_k \mid \mathbf{n}_{0:k}^{(i)}, \mathbf{z}_{1:k-1})\) that the Kalman update produces for free, then normalize.
4. Resample the particles (each carrying its own \((\boldsymbol{\mu}, \boldsymbol{\Sigma})\)) when the effective sample size drops, exactly the diagnostic from Section 10.4.

The one subtlety is that the linear part may couple back into the hard part's dynamics, in which case the Kalman prediction must also account for the sampled \(\mathbf{n}_k^{(i)}\); the bookkeeping is standard but must be gotten right or the exactness is lost.

A minimal marginalized filter

The code below sketches the loop for a mixed model where a discrete mode (the hard part) selects among linear-Gaussian dynamics (the easy part). Each particle holds a mode plus a Kalman mean and covariance; the weight uses the Gaussian innovation likelihood. It is deliberately compact to show the two-layer structure rather than a production tracker.

import numpy as np

def rbpf_step(particles, z, models, resample_thresh=0.5):
    # particles: list of dicts with keys mode, mu, Sigma, w
    for p in particles:
        # 1. propagate the HARD part (discrete mode via a Markov transition)
        p["mode"] = sample_mode_transition(p["mode"], models)
        F, H, Q, R = models[p["mode"]]          # mode fixes the linear model

        # 2. exact Kalman predict + update for the EASY part
        mu_pred = F @ p["mu"]
        S_pred  = F @ p["Sigma"] @ F.T + Q
        y   = z - H @ mu_pred                    # innovation
        S   = H @ S_pred @ H.T + R               # innovation covariance
        K   = S_pred @ H.T @ np.linalg.inv(S)    # Kalman gain
        p["mu"]    = mu_pred + K @ y
        p["Sigma"] = S_pred - K @ H @ S_pred

        # 3. weight by the marginal (Gaussian) likelihood of this measurement
        d = y.shape[0]
        loglik = -0.5 * (y @ np.linalg.inv(S) @ y
                         + np.log(np.linalg.det(S)) + d*np.log(2*np.pi))
        p["w"] *= np.exp(loglik)

    # normalize and (optionally) resample on low effective sample size
    W = np.array([p["w"] for p in particles]); W /= W.sum()
    for p, w in zip(particles, W): p["w"] = w
    n_eff = 1.0 / np.sum(W**2)
    if n_eff < resample_thresh * len(particles):
        particles = systematic_resample(particles, W)
    return particles

The code makes the division of labor concrete: the only stochastic line is the mode draw; every other line is deterministic linear algebra. That is the Rao-Blackwell theorem operationalized, sampling confined to the one variable that has no closed form.

When to Rao-Blackwellize, and when not to

When the technique pays off is precisely when a clean conditional-linear substructure exists: SLAM (map given path), multi-target tracking with unknown data association or maneuver mode, positioning where a sensor bias or clock offset is linear given the trajectory, and any problem where a large slab of the state is a nuisance parameter you only need to integrate out. In these cases the variance reduction is dramatic and free. When it does not help is when almost the entire state is genuinely nonlinear, leaving no linear block worth marginalizing, or when the per-particle Kalman filter is itself expensive (a large \(\mathbf{l}_k\) means an \(O(d_l^3)\) covariance update per particle per step). There is a real trade: you buy statistical efficiency with arithmetic per particle. If the linear block is small, as it is for FastSLAM's independent landmarks, the trade is overwhelmingly favorable; if it is a dense high-dimensional Gaussian, the cubic cost can erase the gain. The estimator-selection guidance of Section 10.7 returns to this budget explicitly.

What's Next

In Section 10.6, we confront the assumption every filter in this chapter has quietly relied on: that the noise is well-behaved and the model is right. We turn to robust and adaptive filtering, handling heavy-tailed outliers, gross sensor faults, and noise statistics that drift while the system runs.