The Estimation Toolbox

1.1State Estimation, Stated Plainly

Chapter 0 left you with a belief that must be predicted forward and corrected by measurements. This chapter builds the machinery that actually does it. First, the vocabulary, because the whole field runs on four objects.

The state x_t is the vector of everything you want to estimate at time t. For a wheeled robot it might be (x, y, θ). For a visual-inertial rig it balloons to position, orientation, velocity, and the IMU's accelerometer and gyro biases: fifteen or so numbers. In full SLAM the state also contains the map, so it can hold thousands of landmark positions. Choosing what goes into the state is a design decision with consequences; every extra dimension costs you later.

The motion model f predicts the next state from the previous one plus a control or odometry input u_t. The measurement model h predicts what a sensor should read if the state were exactly x_t. Both are wrong by a little, and we model that wrongness as additive Gaussian noise. Here is the notation used for the rest of the chapter:

x_t                     state at time t
u_t                     control / odometry input from t-1 to t
z_t                     measurement at time t

x_t = f(x_{t-1}, u_t) + w_t     motion model,       w_t ~ N(0, Q)
z_t = h(x_t)          + v_t     measurement model,  v_t ~ N(0, R)

bel(x_t) = p(x_t | z_{1:t}, u_{1:t})     the belief we maintain

Read the last line slowly. We never estimate a single point. We maintain a probability distribution over states, conditioned on everything we have driven and everything we have seen. Every algorithm in this chapter is a different answer to one question: what shape do you allow bel(x_t) to be?

State

The vector of quantities being estimated: pose at minimum, often velocity, sensor biases, and landmarks too.

Prior / posterior

Your belief before and after absorbing a measurement. The predict step produces a prior; the correct step turns it into a posterior.

Innovation

z_t − h(x̂_t): the gap between what the sensor said and what your state predicted it would say. If your filter is healthy, innovations look like zero-mean noise.

Q and R

Process and measurement noise covariances. Q says "how much do I distrust my motion model", R says "how much do I distrust this sensor". Chapter 2 is about measuring them instead of guessing.

1.2Gaussians and Why Everyone Uses Them

A Gaussian N(μ, Σ) is a bump described by two objects: the mean μ (where the bump sits) and the covariance Σ (how far it spreads, and in which directions). For an n-dimensional state that is n numbers plus a symmetric n×n matrix. Compare that to storing a full arbitrary distribution over a 6-DOF pose, which is hopeless, and you see the first attraction: Gaussians are cheap.

The second attraction is the one that built the field. Push a Gaussian through a linear function and it stays Gaussian. Multiply two Gaussian densities together and the result is proportional to another Gaussian. Since prediction is (approximately) a linear map and Bayesian correction is a multiplication of densities, a Gaussian belief stays Gaussian through the entire predict-correct cycle. You never leave the family, so two small objects, μ and Σ, are all you ever store.

The fusion formula in 1D is worth memorizing because it explains every Kalman update you will ever stare at:

fuse N(μ₁, σ₁²) with N(μ₂, σ₂²):

  σ²  = (1/σ₁² + 1/σ₂²)⁻¹                  precisions ADD
  μ   = σ² · (μ₁/σ₁² + μ₂/σ₂²)             precision-weighted mean

example:  N(3.0, 4.0) × N(5.0, 1.0)  →  N(4.6, 0.8)

Two things to notice in the example. The fused mean 4.6 sits much closer to 5.0, the confident source. And the fused variance 0.8 is smaller than either input. Fusing information never makes you less certain, and the natural currency is precision (inverse covariance), which simply adds.

In more than one dimension, a covariance draws an ellipse (an ellipsoid in 3D+): the contour of equal probability around the mean. Its axes point along the eigenvectors of Σ and their lengths are the square roots of the eigenvalues. A long thin ellipse says "I know my lateral position well but not my forward position", which is precisely what odometry down a featureless corridor produces. Fusion intersects ellipses:

The honest caveat: the world is not Gaussian. Beliefs can be multimodal ("I am in one of these four identical aisles"), and nonlinear models bend Gaussians into banana shapes. Sections 1.4 and 1.5 are two different responses to that caveat.

1.3The Bayes Filter Heartbeat

Chapter 0 gave you the heartbeat in words: predict widens, correct narrows. Here is where the two beats come from, at prose level, no measure theory required.

Start from what we want, p(x_t | z_1:t, u_1:t), and apply Bayes' rule to peel off the newest measurement: the posterior is proportional to p(z_t | x_t) times the belief just before that measurement arrived. That prior belief, in turn, comes from pushing yesterday's posterior through the motion model: sum over every place you could have been at t−1, weighted by how likely the motion u_t was to carry you from there to x_t. Two assumptions make this legal. The Markov assumption: the state summarizes all history, so the past matters only through x_t−1. And conditional independence of measurements given the state. Written compactly:

predict:  bel⁻(x_t) = ∫ p(x_t | x_{t-1}, u_t) · bel(x_{t-1}) dx_{t-1}
correct:  bel(x_t)  = η · p(z_t | x_t) · bel⁻(x_t)

The predict step is a convolution: you take the old belief and smear it by the motion noise. Smearing spreads probability mass, so the belief widens. The correct step is a pointwise multiplication by the measurement likelihood, then renormalization (η). Multiplying by a function that is large where the measurement agrees and small elsewhere concentrates the mass, so the belief narrows. That is the entire derivation, and it is worth being able to say in an interview without notation.

The catch is that the integral is over all possible states. Over a continuous 6-DOF pose you cannot evaluate it exactly, so every practical filter is a compromise about how to represent bel. Constrain it to a Gaussian and you get the Kalman family (1.4). Represent it by samples and you get the particle filter (1.5). Refuse to throw away the past at all and you get least squares and factor graphs (1.6, 1.7).

1.4The Kalman Filter and Its Nonlinear Children

The Kalman filter is the Bayes filter under three restrictions: linear f, linear h, Gaussian noise. Under those, the belief stays exactly Gaussian and both steps are small matrix formulas. Before the matrices, run it by hand.

A Robot on a Rail, Two Steps by Hand

One state variable: position on a rail, in metres. Each step the robot commands +1.0 m of motion. Process noise Q = 0.25 (the drive is sloppy), measurement noise R = 0.5 (the range beacon is sloppier). Start believing x̂ = 0.0 with variance P = 1.0.

Watch the gain K fall from 0.71 to 0.55. At t=1 the filter was ignorant (P = 1.25 against R = 0.5), so it leaned on the sensor. One step later it has history, so it leans less. Keep iterating and P settles where growth by Q exactly balances shrinkage by correction. For this problem you can solve that balance in closed form (p² + pQ − QR = 0) and get p = 0.25, hence a steady-state gain of exactly 0.50. The filter converges to "split the difference", and it would have told you so before seeing a single measurement, because P and K never depend on the data. Only x̂ does.

The general matrix form is the same story with bookkeeping:

predict:  x̂⁻ = F x̂ + B u              P⁻ = F P Fᵀ + Q
correct:  K  = P⁻ Hᵀ (H P⁻ Hᵀ + R)⁻¹                    Kalman gain
          x̂  = x̂⁻ + K (z − H x̂⁻)                        innovation, weighted
          P  = (I − K H) P⁻

The EKF: Linearize and Hope

Real robots rotate, and rotation makes f and h nonlinear (that θ inside sines and cosines). The extended Kalman filter keeps the Kalman machinery but replaces F and H with Jacobians: the matrices of partial derivatives ∂f/∂x and ∂h/∂x, evaluated at the current estimate. Geometrically, you replace the curved model with its tangent plane at the point where you believe you are, and run the linear filter on that plane.

This works remarkably well when the belief is tight, because a small ellipse only sees a small patch of the curve, and small patches look flat. It fails in two characteristic ways. First, strong nonlinearity across the width of the belief: a bearing-only measurement of a nearby landmark curves sharply, the tangent plane misrepresents it, and the update injects error while shrinking P. The filter becomes confidently wrong, which is worse than wrong. Second, bad initialization: the Jacobian is evaluated at your estimate, so if the estimate starts far from the truth, you linearize around fiction and may never recover. EKF divergence is not graceful; P stays small while the error grows.

The UKF, in One Honest Paragraph

The unscented Kalman filter skips Jacobians. It picks 2n+1 sigma points, deterministic samples arranged around the mean along the covariance axes, pushes each one through the true nonlinear function, and refits a Gaussian to where they land. That captures curvature to second order rather than first, costs about the same as an EKF, and spares you deriving derivatives of an ugly model. Prefer it when the dynamics are strongly nonlinear or when the Jacobian is error-prone to write; skip it when the EKF is already consistent, since the answer will barely differ. It is still a Gaussian filter, so it still believes in exactly one place at a time.

sudo apt install python3-venv
python3 -m venv ~/kf-env && source ~/kf-env/bin/activate
pip install numpy matplotlib        # numpy 2.x is fine

#!/usr/bin/env python3
# 1D constant-velocity Kalman filter, position-only measurements.
import numpy as np
import matplotlib.pyplot as plt

rng = np.random.default_rng(42)
dt, n = 0.1, 300                        # 30 seconds at 10 Hz

# simulate truth: a cart with small random accelerations
accel_sigma = 0.3                       # m/s^2
truth = np.zeros((n, 2))                # columns: position, velocity
truth[0] = [0.0, 1.0]                   # starts at x=0 moving 1 m/s
for k in range(1, n):
    a = rng.normal(0, accel_sigma)
    truth[k, 1] = truth[k-1, 1] + a * dt
    truth[k, 0] = truth[k-1, 0] + truth[k-1, 1] * dt + 0.5 * a * dt**2

meas_sigma = 0.5                        # GPS-like: half a metre, 1-sigma
z = truth[:, 0] + rng.normal(0, meas_sigma, n)

F = np.array([[1, dt], [0, 1]])         # constant-velocity model
H = np.array([[1.0, 0.0]])              # we only measure position
Q = accel_sigma**2 * np.array([[dt**4/4, dt**3/2],
                               [dt**3/2, dt**2  ]])
R = np.array([[meas_sigma**2]])

x = np.array([0.0, 0.0])                # deliberately wrong velocity
P = np.diag([1.0, 1.0])
est, gains = np.zeros((n, 2)), np.zeros((n, 2))
for k in range(n):
    x = F @ x                           # predict
    P = F @ P @ F.T + Q
    S = H @ P @ H.T + R                 # correct
    K = P @ H.T @ np.linalg.inv(S)
    x = x + (K @ (z[k] - H @ x)).ravel()
    P = (np.eye(2) - K @ H) @ P
    est[k], gains[k] = x, K.ravel()

print("steady-state gain:", np.round(gains[-1], 3))
print(f"raw RMS error     : {np.sqrt(np.mean((z - truth[:,0])**2)):.3f} m")
print(f"filtered RMS error: {np.sqrt(np.mean((est[:,0]-truth[:,0])**2)):.3f} m")

plt.plot(z, ".", ms=3, alpha=.4, label="raw measurements")
plt.plot(truth[:, 0], "k", lw=1, label="truth")
plt.plot(est[:, 0], "r", lw=1.5, label="KF estimate")
plt.xlabel("step"); plt.ylabel("position [m]"); plt.legend(); plt.show()

1.5The Particle Filter

When the belief genuinely has several bumps, a Gaussian is the wrong container. The particle filter represents bel(x_t) as a set of N samples (particles), each a full candidate state, each carrying an importance weight. The cloud of particles is the distribution; wherever the cloud is dense, probability is high.

The loop mirrors the Bayes filter directly. Predict: push every particle through the motion model with fresh sampled noise, so the cloud spreads. Correct: weight each particle by the measurement likelihood p(z_t | x^[i]), so particles that explain the scan get heavy and particles behind a wall get light. Resample: draw a new generation of N particles with probability proportional to weight, so heavy particles duplicate and light ones die. Without resampling, after a few hundred steps one particle holds nearly all the weight and the other 4,999 are payroll with no output. The standard implementation is low-variance (systematic) resampling, one random number and a comb of N evenly spaced pointers, and the standard trigger is resampling only when the effective sample size N_eff = 1/Σw² drops below about N/2.

Resampling has a dark side called particle deprivation: every resample deletes diversity, and duplicated particles do not spread back out until motion noise separates them. Resample too eagerly in an uninformative stretch and the cloud collapses onto one hypothesis that later proves wrong, with no survivors near the truth. This is why the N_eff test exists, and why implementations add a small trickle of uniformly scattered particles as insurance against the kidnapped-robot case.

Why does 2D lidar localization love this filter? Because the state is only (x, y, θ), and a few thousand particles cover 3 dimensions densely. The likelihood is cheap (raycast the map, compare ranges), multimodality is handled for free, and global localization just means initializing particles everywhere. That recipe is Monte Carlo Localization, which Chapter 9 builds from scratch in Python. Why does 6-DOF SLAM not love it? The particle count needed grows roughly exponentially with dimension. Covering 6 pose dimensions plus velocities and biases would take millions of particles, and each one would need its own copy of the map. Above three or four dimensions, everyone retreats to Gaussians or graphs.

FastSLAM (Montemerlo et al., 2002) is the historical bridge and the clever exception. Each particle carries a full trajectory hypothesis plus its own map, but the map is not sampled: conditioned on a known trajectory, landmark estimates become independent, so each particle maintains one tiny EKF per landmark. Sampling only the pose and solving the map analytically is Rao-Blackwellization: sample the hard variable, integrate the easy ones in closed form. The grid-map version of the same idea is GMapping, which built a decade of 2D robot maps before graph optimizers took over.

1.6From Filters to Least Squares

A filter marginalizes out the past at every tick. A loop closure, though, is information about the past: it says pose 200 equals pose 12. A filter can only push that correction into its current state; the trajectory already logged stays bent. The alternative is to keep past poses as unknowns and solve for all of them at once. This is smoothing, and its natural language is least squares.

Here is the maximum-likelihood view. Each measurement i (an odometry edge, a loop closure, a landmark sighting) gives a residual r_i(x) = z_i − h_i(x): the disagreement between what was measured and what the candidate solution x predicts. If measurement noise is Gaussian, the likelihood of measurement i is exp(−½ r_iᵀ Σ_i⁻¹ r_i), and maximizing the product of likelihoods over all measurements is exactly minimizing the sum of squared, covariance-weighted residuals. Chapter 0's spring picture is this equation wearing overalls: each residual is a stretched spring, Σ_i⁻¹ is its stiffness, and the solution is the pose set of minimum stored energy.

x* = argmin_x  Σᵢ  rᵢ(x)ᵀ Σᵢ⁻¹ rᵢ(x)

linearize each rᵢ around a guess, stack Jacobians J, solve

(Jᵀ Σ⁻¹ J) δx = −Jᵀ Σ⁻¹ r          the normal equations
x ← x ⊞ δx, repeat until δx is tiny

The matrix H = JᵀΣ⁻¹J is the SLAM Hessian (strictly, the Gauss-Newton approximation of it), and its saving grace is sparsity. Each residual touches at most a couple of variables: an odometry edge involves two consecutive poses, a landmark observation involves one pose and one landmark. So J has a handful of nonzero blocks per row, and H is nearly all zeros. A trajectory of 10,000 poses gives a Hessian with 10⁸ entries, of which perhaps 10⁵ are nonzero, and sparse Cholesky factorization eats that for breakfast. Without sparsity, graph SLAM would still be a curiosity.

Two iteration schemes solve this in practice. Gauss-Newton trusts the linearization completely and takes the full step δx; near the solution it converges fast, far from it the step can overshoot into a worse place. Levenberg-Marquardt adds a damping term λ to the diagonal, solving (H + λI)δx = −g. The intuition is line search versus trust region: Gauss-Newton picks a direction and commits, while LM asks "over what radius do I trust my quadratic model?", shrinking λ when steps help (behaving like Gauss-Newton) and growing it when steps hurt (behaving like cautious gradient descent). LM is the default in every SLAM back-end for a reason: pose graphs after a big loop closure are exactly the far-from-solution regime where Gauss-Newton lunges.

1.7Factor Graphs, the Modern Back-End

Draw the least-squares problem instead of writing it. Circles are variables (poses, landmarks, biases). Squares are factors, one per measurement, each attached to only the variables it constrains. This picture is the factor graph, and it is the standard language of modern SLAM back-ends.

The graph is sparse for the same reason H was: measurements are local. Solving it means elimination: remove variables one at a time, each removal passing that variable's information to its neighbors, which is Gaussian elimination viewed graph-side. Eliminating a variable connects all its neighbors to each other, adding edges that were not there before. That added density is fill-in, and the elimination order controls how much of it you suffer; heuristics like COLAMD pick orderings that keep the factorization sparse. Eliminate badly and a sparse problem turns dense halfway through.

Marginalization is elimination you never undo: integrate a variable out and keep only its effect on its neighbors. The Gaussian math is the Schur complement, and the practical consequence matters more than the formula: marginalizing a variable ties all of its neighbors together with a dense prior. Fixed-lag smoothers (a sliding window of the most recent poses, everything older marginalized) live with this bargain: bounded compute, at the cost of dense blocks and permanently baked-in linearization points at the window's trailing edge. Every VIO system in Chapter 4 sits somewhere on this dial between filter (window of 1) and full smoother (window of everything).

iSAM2 (Kaess et al., 2012) made full smoothing incremental. It keeps the factorization in a Bayes tree and, when a new factor arrives, re-eliminates only the cliques the factor touches, relinearizing variables only when their estimate has moved enough to matter. A new odometry edge disturbs a leaf; a big loop closure disturbs a large subtree and costs accordingly. That is the honest cost model of modern SLAM: cheap most of the time, expensive exactly when the map changes shape.

python3 -m venv ~/gtsam-env && source ~/gtsam-env/bin/activate
pip install gtsam numpy
python -c "import gtsam; print(gtsam.__version__)"   # expect 4.2.x

#!/usr/bin/env python3
# 5-pose 2D pose graph: drifting odometry + one loop closure.
import numpy as np
import gtsam

graph = gtsam.NonlinearFactorGraph()

# noise models, sigmas are [x, y, theta]
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.005]))
odom_noise  = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.10, 0.10, 0.05]))
loop_noise  = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.02]))

# pin pose 0, otherwise the whole graph can float (gauge freedom)
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(0, 0, 0), prior_noise))

# the robot drove a 2 m square, but odometry reports 2.1 m legs
# and 84.8 degree turns instead of 90: classic drift
meas_odom = gtsam.Pose2(2.1, 0.0, 1.48)
initial = gtsam.Values()
pose = gtsam.Pose2(0, 0, 0)
initial.insert(0, pose)
for i in range(4):
    graph.add(gtsam.BetweenFactorPose2(i, i + 1, meas_odom, odom_noise))
    pose = pose.compose(meas_odom)      # integrate odometry as the guess
    initial.insert(i + 1, pose)

# loop closure: the front-end recognized that pose 4 is pose 0
graph.add(gtsam.BetweenFactorPose2(4, 0, gtsam.Pose2(0, 0, 0), loop_noise))

params = gtsam.LevenbergMarquardtParams()
result = gtsam.LevenbergMarquardtOptimizer(graph, initial, params).optimize()

for i in range(5):
    b, a = initial.atPose2(i), result.atPose2(i)
    print(f"x{i}  before ({b.x():+.2f}, {b.y():+.2f}, {b.theta():+.2f})"
          f"   after ({a.x():+.2f}, {a.y():+.2f}, {a.theta():+.2f})")
print("graph error before:", round(graph.error(initial), 2))
print("graph error after :", round(graph.error(result), 2))

1.8Robust Kernels and Bad Loop Closures

Least squares has a temper. The cost of a residual grows with its square, so a measurement that is wrong by 10σ contributes 100 times the cost of a 1σ one, and the optimizer will happily bend the entire trajectory to appease it. Gaussian noise makes big residuals nearly impossible, so the model assumes any big residual must be fixed at any price. But loop closures do not fail Gaussianly. A place-recognition front-end that mistakes aisle 3 for aisle 6 produces a constraint that is not noisy but simply false, and under a quadratic cost one false constraint can fold a city-block map in half.

Robust kernels replace the quadratic ρ(r) = r² with a function that stops escalating. Huber stays quadratic inside a threshold δ and grows linearly beyond it: inliers are treated exactly as before, outliers keep a constant pull instead of a growing one. Cauchy goes further and flattens, so a gross outlier is nearly ignored, at the price of a non-convex cost with more local minima. In implementation terms, each kernel is an iteratively reweighted least squares: big residuals get their information matrix scaled down every iteration, which is why dynamic covariance scaling (Agarwal et al., 2013) can express the same protection as a closed-form per-factor covariance inflation. Switchable constraints (Sünderhauf and Protzel, 2012) make the choice explicit: every loop closure gets a switch variable the optimizer itself can turn toward zero, with a prior nudging switches to stay on, so the graph pays a small fee to disable a constraint it cannot reconcile.

1.9Rotations Need Special Care

Everything so far quietly assumed states live in a vector space: you can add a correction δx to a state and covariances describe spreads along coordinate axes. Position behaves that way. Rotation does not, and pretending it does is behind a remarkable fraction of broken SLAM code.

Euler angles (roll, pitch, yaw) are three numbers, so they look like a vector, but the disguise fails three ways. Rotations do not commute: roll-then-pitch is not pitch-then-roll, while vector addition never cares about order. Angles wrap: the average of 359° and 1° is 0°, not 180°, and any code that averages or interpolates raw angles will eventually produce the 180° answer. And at pitch ±90° two of the three axes align and a degree of freedom vanishes: gimbal lock, where the parametrization (not the physics) becomes singular and Jacobians blow up. Fine for reading a display, unsafe for estimation.

The clean mental model: the set of 3D rotations, SO(3), is a curved three-dimensional surface (a manifold) living inside those higher-dimensional representations, the way the Earth's surface is a curved 2D thing inside 3D space. Rigid poses, rotation plus translation, form SE(3), a six-dimensional manifold. You cannot add points on a curved surface, but you can zoom in: locally, every manifold looks flat, and that local flat approximation is the tangent space, an honest ℝ³ (or ℝ⁶) where addition, Gaussians, and least squares all work.

The notation for this is boxplus and boxminus. x ⊞ δ means "perturb the manifold point x by the tangent vector δ, then map back onto the manifold" (for rotations: convert δ to a small rotation via the exponential map and compose). y ⊟ x is the inverse: the small tangent vector that carries x to y. With these two operators the whole chapter transfers unchanged: the Gauss-Newton update in 1.6 already said x ← x ⊞ δx, the covariance is a plain 3×3 or 6×6 matrix over the tangent space, and no constraint or singularity ever appears because δ lives near zero, far from any wraparound. This is exactly how GTSAM, g2o, and Ceres handle rotations internally, and it is why the error-state Kalman filter parametrizes the IMU state by a small error rather than the rotation itself. The reading list has the two Solà papers that make this precise without demanding a differential geometry course.

SO(3), SE(3)

The groups of 3D rotations and of rigid transforms (rotation + translation). "Group" here mostly means: composable, invertible, and closed under both.

Manifold

A curved space that looks flat up close. Sphere: 2D manifold. SO(3): 3D manifold. Flat-up-close is the property optimizers exploit.

Tangent space

The flat vector space that best approximates the manifold at one point. Where δx, Gaussians, and Jacobians live.

Exponential / log map

The round trips between tangent space and manifold: exp wraps a tangent vector onto the manifold, log unwraps a manifold displacement into a vector.

1.10Reading List

1.11Interview Questions