Math 3 · Probability & estimation

Why this chapter exists. Sensors are noisy, so a robot never knows its state. It holds a belief: a probability distribution over states. GTSAM's job is to compute the most probable state given all measurements. This chapter goes from random variables to the SLAM posterior and shows that "most probable state with Gaussian noise" is exactly nonlinear least squares. That single fact is why GTSAM is an optimizer. Along the way you'll see that conditioning and marginalizing Gaussians are the Schur-complement operations of chapter 1, which is why factor-graph elimination (chapter 5) works.

1. Random variables, expectation, covariance

A continuous random vector \(x\in\R^n\) has a density \(p(x)\ge0\) with \(\int p(x)\,dx=1\). Its mean is \(\mu = \E[x] = \int x\,p(x)\,dx\), and its covariance is

\(\Sigma\) is symmetric and positive semidefinite, since \(v^\top\Sigma v = \E[(v^\top(x-\mu))^2]\ge0\). Its diagonal holds the variances \(\sigma_i^2\). Correlation is \(\rho_{ij} = \Sigma_{ij}/(\sigma_i\sigma_j)\in[-1,1]\).

Linear maps. If \(y = Ax+b\), then \(\E[y] = A\mu+b\) and \(\operatorname{cov}(y) = A\Sigma A^\top\) (expand the definition). This one rule propagates uncertainty through every linearized function: \(\Sigma_y\approx J\Sigma_xJ^\top\).

2. The multivariate Gaussian

The exponent is \(-\tfrac12\lVert x-\mu\rVert^2_\Sigma\), the squared Mahalanobis distance. It measures distance in units of standard deviations along each principal axis. Level sets are ellipsoids with axes along the eigenvectors of \(\Sigma\) and semi-axes \(\propto\sqrt{\lambda_i}\), which is how 1σ/2σ/3σ uncertainty ellipses are drawn.

Why Gaussians everywhere?

1. Central limit theorem: sums of many small independent errors tend to Gaussian.
2. Maximum entropy: among all distributions with a given mean and covariance, the Gaussian assumes the least extra structure.
3. Closure: linear maps, products, marginals, and conditionals of Gaussians are Gaussian, all in closed form. That makes exact inference cheap.

3. Information (canonical) form

Expand the exponent, \(-\tfrac12x^\top\Sigma^{-1}x + x^\top\Sigma^{-1}\mu + \text{const}\), and name the pieces:

Multiplying Gaussians adds their information. If \(p_1\propto\exp(-\tfrac12x^\top\Lambda_1x+\eta_1^\top x)\) and similarly for \(p_2\), then \(p_1p_2\propto\exp(-\tfrac12x^\top(\Lambda_1+\Lambda_2)x+(\eta_1+\eta_2)^\top x)\). Fusing independent evidence is addition. This is why HessianFactors are combined by adding their blocks, and why least-squares terms add.

Information is additive, covariance isn't. Which form is convenient depends on the operation:

4. Marginals and conditionals of a partitioned Gaussian

Split \(x = (a, b)\):

Conditional \(p(a\mid b)\) in information form. Fix \(b\) and keep only the terms involving \(a\): \(-\tfrac12a^\top\Lambda_{aa}a - a^\top\Lambda_{ab}b + \eta_a^\top a\). This is a Gaussian in \(a\) with

The conditional mean is a linear function of \(b\), and the conditional covariance \(\Lambda_{aa}^{-1}\) doesn't depend on \(b\) at all.

Marginal \(p(b)\) in information form. Integrate \(a\) out. Complete the square in \(a\) and the leftover quadratic in \(b\) is

This is the Schur complement of chapter 1 §10, with exactly the same algebra. Covariance form is dual: the marginal is \(\Sigma_{bb}\) (trivial), and the conditional is \(\mu_{a\mid b} = \mu_a+\Sigma_{ab}\Sigma_{bb}^{-1}(b-\mu_b)\), \(\Sigma_{a\mid b} = \Sigma_{aa}-\Sigma_{ab}\Sigma_{bb}^{-1}\Sigma_{ba}\).

Why this matters more than anything else in the chapter. The chain rule \(p(a,b) = p(a\mid b)\,p(b)\), computed in information form, is

That is one step of variable elimination in a factor graph, and it is one step of Cholesky (chapter 1 §9). GTSAM's EliminateCholesky returns exactly this pair: a GaussianConditional and a HessianFactor on the separator. Chapter 5 runs it repeatedly.

5. Bayes' rule, likelihood, MAP

• Measurement model: \(z = h(x) + \nu\) with \(\nu\sim\mathcal N(0,\Sigma)\) gives the likelihood \(p(z\mid x) = \mathcal N(z; h(x),\Sigma)\). Viewed as a function of \(x\) with \(z\) fixed, this is a factor \(\phi(x)\).
• Maximum likelihood (ML): \(\argmax_x p(z\mid x)\), with no prior.
• Maximum a posteriori (MAP): \(\argmax_x p(x\mid z) = \argmax_x p(z\mid x)p(x)\), because \(p(z)\) doesn't depend on \(x\). The prior regularizes the solution and fixes gauge freedom (chapter 1 §2).

6. From MAP to nonlinear least squares: the one derivation that explains GTSAM

Assume many measurements \(z_k\), each depending on a small subset of variables \(X_k\subset X\), with independent Gaussian noise. Independence lets the likelihood factor:

Since \(-\log\) is monotonically decreasing, maximizing the product means minimizing the sum of negative logs (the normalizing constants don't depend on \(X\)):

(for manifold-valued measurements, the difference \(\ominus\) is the local-coordinates map from chapter 2). This is nonlinear least squares. Each term is one factor; GTSAM calls its value factor.error(values), and graph.error(values) is the whole sum. Every piece of a GTSAM factor maps to a piece of this formula:

7. The Laplace approximation: where covariances come from

Near the MAP estimate \(\hat X\), linearize, \(h(\hat X\oplus\delta)\approx h(\hat X)+J\delta\), with \(J\) the stacked whitened Jacobian. Then \(-\log p(\hat X\oplus\delta\mid Z)\approx\text{const}+\tfrac12\delta^\top J^\top J\delta\), so

The posterior is approximated by a Gaussian in the tangent space at the optimum, with information matrix \(J^\top J\) (the Gauss–Newton Hessian). gtsam::Marginals computes blocks of \((J^\top J)^{-1}\) without forming the full inverse (chapter 5 §13). The approximation is good when the problem is nearly linear over the uncertainty region (small noise, well-constrained), and poor for highly ambiguous problems, e.g. range-only measurements with multimodal posteriors.

8. Recursive estimation: the Kalman filter as elimination

State-space model:

Draw the factor graph: a prior on \(x_{k-1}\) (mean \(\mu\), covariance \(P\)), a motion factor between \(x_{k-1}\) and \(x_k\), and a measurement factor on \(x_k\).

Predict = eliminate \(x_{k-1}\). The joint over \((x_{k-1},x_k)\) is Gaussian. Marginalizing \(x_{k-1}\), easiest in covariance form with \(x_k = Fx_{k-1}+w\), gives

Update = multiply in the measurement factor (information form, so just add):

Applying the Woodbury identity (chapter 1 §10) converts this into the familiar gain form:

So the Kalman filter is variable elimination on a chain-shaped factor graph, eliminating the oldest state at each step. gtsam/linear/KalmanFilter.h is implemented exactly this way: it builds small Gaussian factor graphs and eliminates them.

EKF. For nonlinear \(f, h\), linearize at the current estimate (\(F = \partial f/\partial x\), \(H = \partial h/\partial x\)) and apply the same formulas. On manifolds, use the error-state form: the filter's state is a tangent-space perturbation and the mean is retracted after each update. GTSAM has ExtendedKalmanFilter (factor-graph based), plus ManifoldEKF, LieGroupEKF, and InvariantEKF (where the error is defined via the group so its dynamics are state-independent), as well as EquivariantFilter.

Filtering's fundamental limitation. Once \(x_{k-1}\) is eliminated, its linearization point is frozen forever. If you later learn the robot was elsewhere (a loop closure), the earlier linearization error can't be undone. Smoothing fixes this.

9. The SLAM posterior and why smoothing wins

Variables: poses \(x_0,\dots,x_T\) and landmarks \(l_1,\dots,l_M\). Models: motion \(x_t = f(x_{t-1},u_t)+w_t\) and observation \(z_k = h(x_{i_k}, l_{j_k}) + v_k\).

As a factor graph, take three poses and two landmarks, where \(l_1\) is seen from \(x_1\) and \(x_2\) and \(l_2\) from \(x_3\) (GTSAM's PlanarSLAMExample):

   (p)──x1────(o)────x2────(o)────x3
         \          /              \
         (m)      (m)              (m)
           \      /                  \
             l1                       l2
   (p)=prior  (o)=odometry  (m)=measurement factor

The information matrix \(\Lambda = J^\top J\) has a nonzero block wherever two variables share a factor. Order the blocks \(x_1, x_2, x_3, l_1, l_2\):

It is sparse: the number of nonzero blocks grows linearly with the number of measurements.

Now filter instead. Online SLAM keeps only the current pose plus the landmarks, marginalizing each past pose as soon as the next one arrives. Marginalizing a pose is a Schur complement, which connects all of that pose's neighbours to each other. After a while, every landmark is connected to every other landmark: \(\Lambda\) fills in and \(\Sigma\) is fully dense. That is EKF-SLAM's \(O(M^2)\) memory and update cost, and its frozen linearization points.

Smoothing keeps all the poses (or a window of them). The problem gets bigger but stays sparse, and every variable can be re-linearized whenever the estimate improves. Sparse factorization of a bigger sparse problem is far cheaper than a dense filter update, and incremental methods (iSAM2) make each new measurement cost nearly constant time. This is the founding argument of the "Square Root SAM" line of work that became GTSAM (Dellaert & Kaess 2006).

Loop closures add a factor between \(x_i\) and \(x_j\) with \(\lvert i-j\rvert\) large. That adds one off-diagonal block, but during elimination it creates fill along the loop (chapter 5).

10. Observability and gauge freedom, in probability terms

If the likelihood is unchanged when you apply some transformation \(g\) to all variables, then \(\Lambda\) is singular along those directions. The posterior is flat there, and the MAP is not unique.

Fix these by adding priors (a strong prior on \(x_0\)), by anchoring (NonlinearEquality), or by choosing a gauge (ShonanGaugeFactor). Don't fix more directions than are unobservable. In VIO, a prior on roll and pitch at \(x_0\) that's tighter than the IMU justifies biases the whole trajectory.

11. Probabilistic graphical models: three ways to draw a factorization

• Bayes network (directed acyclic graph): \(p(X) = \prod_i p(x_i\mid\text{parents}(x_i))\). It shows generative structure, i.e. how the data was produced. Elimination produces a Bayes net (chapter 5).
• Markov random field (undirected): \(p(X)\propto\prod_c\psi_c(X_c)\) over cliques. Its edges are the sparsity pattern of \(\Lambda\). Two variables are conditionally independent given a set \(S\) iff \(S\) separates them in the graph.
• Factor graph (bipartite variable/factor nodes): \(p(X)\propto\prod_k\phi_k(X_k)\). It is the most explicit of the three: it shows which measurement ties which variables, which the MRF loses. It is the structure GTSAM builds (NonlinearFactorGraph).

Conditional independence is the source of sparsity. In the SLAM example, \(x_1\) and \(x_3\) are independent given \(x_2\) (if no other factor links them), and that independence is the zero block \(\Lambda_{13}=0\).

12. Robust estimation: when the Gaussian assumption is wrong

The problem. A Gaussian has very light tails. The cost of a residual grows like \(r^2\), so a single wrong data association (a bad loop closure, a mismatched feature 50σ away) contributes \(50^2=2500\) units of cost and drags the solution far off. Real front-ends always produce some outliers.

M-estimators replace \(\tfrac12r^2\) with a function \(\rho(r)\) that grows more slowly:

Iteratively Reweighted Least Squares (IRLS). Set the gradient to zero: \(\sum_k\rho'(r_k)\,\nabla r_k = \sum_k\underbrace{\tfrac{\rho'(r_k)}{r_k}}_{w(r_k)}\,r_k\nabla r_k = 0\). This is the stationarity condition of a weighted least-squares problem \(\sum_k\tfrac12w_kr_k^2\) with the weights \(w_k=w(r_k)\) frozen. Alternate between computing weights at the current estimate and solving the weighted least squares. GTSAM does this inside every linearization: noiseModel::Robust::WhitenSystem first whitens, then multiplies the rows of \(A\) and \(b\) by \(\sqrt{w(\lVert r\rVert)}\) (gtsam/linear/LossFunctions.cpp, reweight), so the optimizer never knows robust losses exist.

The kernels, exactly as implemented in gtsam/linear/LossFunctions.cpp (\(k\) or \(c\) is the tuning threshold, in units of σ since \(r\) is whitened):

The trade-off. Bounded (redescending) kernels reject gross outliers completely but are non-convex: from a bad initial guess they can lock in a wrong solution, where the true inliers look like outliers. Convex kernels (Huber) are safe but only damp outliers. Graduated non-convexity (chapter 4 §9) gets the best of both.

Gating with χ². If the model is right, the squared whitened residual of a \(d\)-dimensional measurement follows a \(\chi^2_d\) distribution. A 95% gate rejects measurements with \(\lVert r\rVert^2\) above 3.84 (\(d=1\)), 5.99 (\(d=2\)), 7.81 (\(d=3\)), or 12.59 (\(d=6\)). GTSAM's GNC uses the inverse χ² CDF (nonlinear/internal/ChiSquaredInverse.h) to set its inlier threshold.

13. Worked application: IMU preintegration

An IMU gives angular rate \(\tilde\omega\) and specific force \(\tilde a\) at 100–1000 Hz. Camera or lidar keyframes come at 10–30 Hz. Adding a state per IMU sample would make the graph enormous, so preintegration compresses all samples between two keyframes \(i\) and \(j\) into one factor.

Sensor model (body frame, biases \(b_g, b_a\), white noise \(\eta\)):

where \(a\) is the world-frame acceleration and \(g\) the gravity vector. Kinematics: \(\dot R = R\skew{\omega}\), \(\dot v = a\), \(\dot p = v\). Integrating over \([t_i,t_j]\) with sample period \(\Delta t\):

Problem. The sums contain \(R_k\), the absolute orientation at every sample, so whenever the optimizer changes \(R_i\) you would have to re-integrate everything.

Trick. Move everything that depends on the state at \(i\) to one side and define the preintegrated increments, which depend only on the measurements and the bias:

The factor residuals compare these measured increments against the ones predicted by the states:

Noise. Propagate the 9×9 covariance of the increments sample by sample, \(\Sigma_{k+1} = A_k\Sigma_kA_k^\top + B_k\Sigma_\eta B_k^\top\), where \(A_k, B_k\) are the Jacobians of one integration step (chapter 2 §13).

Bias changes. If the bias estimate changes by a small \(\delta b\), update to first order instead of re-integrating. For example, \(\Delta\tilde R_{ij}(\bar b_g+\delta b_g)\approx\Delta\tilde R_{ij}(\bar b_g)\Exp\big(\tfrac{\partial\Delta R}{\partial b_g}\delta b_g\big)\), where the bias Jacobians are accumulated during integration.

In GTSAM (gtsam/navigation): PreintegrationParams holds gravity (MakeSharedU(g) gives \((0,0,-g)\) for z-up/ENU, MakeSharedD(g) gives \((0,0,+g)\) for NED), noise densities, and an optional body-to-IMU pose. TangentPreintegration (the default) stores the increment as a 9-vector in the tangent space of NavState, ordered (θ, position, velocity), together with 9×3 Jacobians with respect to each bias (preintegrated_H_biasAcc_, preintegrated_H_biasOmega_). ImuFactor ties \((x_i,v_i,x_j,v_j,b)\), while CombinedImuFactor also models bias random walk between \(b_i\) and \(b_j\). Forster et al. (TRO 2017) is the reference derivation; navigation/doc/ has GTSAM's own notes.

14. Check yourself

1. Derive the information-form conditional and marginal of a partitioned Gaussian by completing the square.
2. Show that the Kalman update in information form becomes the gain form via Woodbury.
3. In the PlanarSLAM example, marginalize \(x_2\) first. Which new nonzero blocks appear in \(\Lambda\)?
4. Why does a 3D pose graph need a prior, while a VIO problem only needs priors on position and yaw?
5. Derive the IRLS weight \(w = \rho'(r)/r\) for Cauchy and confirm it matches the table.
6. Why do the preintegrated increments not depend on \(R_i\), \(v_i\), or \(p_i\)?