From filters to factor graphs

"A filter remembers the present and forgets how it got there. A factor graph keeps the receipts. When a late measurement contradicts an old belief, only one of them can revise history."

A Retrospective AI Agent

The cost of forgetting: filtering versus smoothing

A recursive filter answers one question: given all measurements up to and including now, what is the state now? Formally it tracks the filtering distribution \(p(\mathbf{x}_k \mid \mathbf{z}_{1:k})\). To stay recursive it marginalizes out \(\mathbf{x}_{k-1}\) the instant it moves to step \(k\), folding the old state into the process-noise-inflated prior for the new one. That marginalization is the source of both its speed and its amnesia. Once \(\mathbf{x}_{k-1}\) is gone, no future measurement can ever correct it, and no future evidence can revisit the linearization point that was used to fold it in.

Smoothing asks the larger question: given all measurements over some window, what is the entire trajectory \(p(\mathbf{x}_{0:K} \mid \mathbf{z}_{1:K})\)? Here nothing is marginalized away during processing. Every state stays an explicit unknown, so a measurement at step \(K\) can and does sharpen the estimate of \(\mathbf{x}_5\), and every nonlinear term can be re-linearized around the improved estimate rather than the first guess. The price is that you now solve for many states jointly instead of one at a time. The factor graph is the data structure that makes that joint problem tractable, because the joint posterior is not a dense tangle: it factors.

The joint posterior factors into local terms

Why can we hope to solve for hundreds of states at once? Because the measurement model is local. Odometry couples only consecutive poses; a range reading couples one pose to one landmark; a prior touches one state. By Bayes' rule the posterior over the whole trajectory is proportional to the joint, and the joint splits into a product of these local pieces,

where \(\mathbf{X}\) collects all unknowns, each factor \(\phi_i\) is a nonnegative function of only the small subset \(\mathbf{X}_i\) of variables it actually constrains, and the constant of proportionality is the evidence we never need. A factor is nothing more mysterious than one term in this product: a prior, a motion constraint, or a sensor likelihood. For the Gaussian noise models of Chapter 2, each factor is \(\phi_i(\mathbf{X}_i) \propto \exp\!\big(-\tfrac12 \lVert h_i(\mathbf{X}_i) - \mathbf{z}_i \rVert^2_{\Sigma_i}\big)\), so taking the negative logarithm turns the product into a sum of squared, noise-weighted residuals. That sum is the objective Section 11.2 minimizes; here we only need to see that it exists and is a sum of local pieces.

Anatomy of a factor graph

A factor graph is the bipartite graph of that factorization. It has two kinds of nodes. Variable nodes are the unknowns you want to estimate: robot poses, landmark positions, sensor biases, a clock offset. Factor nodes are the constraints, one per \(\phi_i\), and each factor node connects by an edge to exactly the variables in its argument \(\mathbf{X}_i\). There are no variable-to-variable or factor-to-factor edges. Reading the graph tells you everything about coupling: a variable's degree is how many measurements see it, and two variables share information only through a common factor. A dead-reckoning chain (Chapter 24) is a line of pose variables linked by odometry factors, with a prior factor pinning the first pose so the whole chain has an absolute reference.

This sparsity is not cosmetic. The negative-log-posterior is quadratic (after linearization), and its Hessian, the information matrix \(\boldsymbol{\Lambda}\), has a nonzero block for two variables exactly when a factor connects them. A chain of poses linked only to their neighbors produces a block-tridiagonal \(\boldsymbol{\Lambda}\): almost entirely zeros. The graph is the sparsity pattern, and that is what lets a smoother scale to thousands of variables instead of choking on a dense inverse. The snippet below assembles the information matrix for a short pose chain and shows the structure directly.

import numpy as np

# Tiny pose chain: 4 scalar states, 1 anchor prior + 3 odometry factors.
# Each factor contributes a rank-1 (or rank-2) block to the information matrix.
n = 4
Lambda = np.zeros((n, n))

def add_prior(Lambda, i, info):          # prior on x_i: touches one variable
    Lambda[i, i] += info

def add_between(Lambda, i, j, info):     # constraint x_j - x_i = odom: touches two
    J = np.zeros(n); J[i], J[j] = -1.0, 1.0
    Lambda += info * np.outer(J, J)

add_prior(Lambda, 0, 100.0)              # anchor the first pose strongly
for k in range(n - 1):                   # odometry links consecutive poses
    add_between(Lambda, k, k + 1, 10.0)

print((Lambda != 0).astype(int))         # sparsity: block-tridiagonal

Run it and the nonzero pattern is a narrow band hugging the diagonal, precisely mirroring the chain in the graph. Add one factor linking pose 3 back to pose 0 (a loop closure, the subject of Section 11.4) and two off-diagonal corners light up: the graph edge and the matrix entry are the same fact stated twice.

Filtering is elimination on the same graph

The unifying payoff: a Kalman filter and a batch smoother operate on the identical factor graph. They differ only in the order and aggressiveness of variable elimination. Solving a linear-Gaussian factor graph means eliminating variables one at a time, and eliminating a variable marginalizes it out, which introduces a new factor coupling all of its former neighbours (this induced factor is called a fill-in). A filter eliminates variables in strict time order and, crucially, discards each eliminated variable's node the moment it is gone, so at any instant it stores only the frontier: one belief over the current state. That frontier belief is exactly the Kalman filter's mean and covariance. A smoother runs the same elimination but retains every variable, so it can eliminate in a cheaper order and, when new evidence arrives, un-eliminate and re-solve. The recursive filter you already know is thus a special-purpose graph solver that happens to keep only a moving window of size one.

import gtsam
from gtsam import Pose2, BetweenFactorPose2, PriorFactorPose2

graph = gtsam.NonlinearFactorGraph()
prior_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
odom_noise  = gtsam.noiseModel.Isotropic.Sigma(3, 0.2)

graph.add(PriorFactorPose2(0, Pose2(0, 0, 0), prior_noise))   # anchor pose 0
for k in range(3):                                            # odometry links
    graph.add(BetweenFactorPose2(k, k + 1, Pose2(1, 0, 0), odom_noise))

# graph now encodes the same factorization; solving is one call (Section 11.2)

What's Next

In Section 11.2, we turn the graph into an objective and solve it. Taking the negative logarithm of the product of Gaussian factors yields a sum of squared, noise-weighted residuals, and estimating the trajectory becomes a sparse nonlinear least-squares problem. We derive the maximum-a-posteriori estimate, linearize with the same Jacobians from Chapter 10, and see why Gauss-Newton and Levenberg-Marquardt, running on the sparse structure this section exposed, solve for thousands of states faster than intuition suggests.