Nonlinear Least Squares

The least-squares method is one of the cornerstones of modern estimation and optimization in robotics. It provides a simple yet powerful way to find the state that best fits a set of noisy measurements.

Least squares tries to minimize the difference between what we measure and what we expect to measure.

• Originally used decades ago, but computationally too expensive for large systems.
• With the rise of efficient solvers (gtsam, g2o, iSAM2) and sparse linear algebra in the 2010s, it made a strong comeback in SLAM and computer vision.
• Today it is the foundation of most graph-based SLAM, bundle adjustment, and trajectory optimization techniques.

Least squares in general

The least-squares method is designed to compute a solution for an overdetermined system ("more equations than unknowns").

Goal. Minimize the sum of squared errors across the equations. Standard approach for a large class of problems — for instance, in regression we fit a line or curve that best matches observed data.

Problem definition

Given a system described by a set of $n$ observation functions $\{f_i(\mathbf{x}){\}}_{i=1:n}$:

• $\mathbf{x}$ is the state vector,
• ${\mathbf{z}}_i$ is a measurement of the state $\mathbf{x}$,
• ${\hat{\mathbf{z}}}_i=f_i(\mathbf{x})$ is a function which maps $\mathbf{x}$ to a predicted measurement.

Given $n$ noisy measurements ${\mathbf{z}}_{1:n}$, estimate the state $\mathbf{x}$ that best explains them.

Error function

The error ${\mathbf{e}}_i$ is typically the difference between the predicted and actual measurement:

$${\mathbf{e}}_i(\mathbf{x})={\mathbf{z}}_i-f_i(\mathbf{x})$$

Assume the error has zero mean and is normally distributed — a Gaussian with information matrix ${\mathbf{\Omega }}_i$. The squared error of a measurement depends only on the state and is a scalar:

$$e_i(\mathbf{x})={\mathbf{e}}_i(\mathbf{x}{)}^T{\mathbf{\Omega }}_i{\mathbf{e}}_i(\mathbf{x})$$

Find the minimum

Find the state ${\mathbf{x}}^{\star }$ that minimizes the error over all measurements:

$$\begin{array}{rl}{\mathbf{x}}^{\star }& =\arg \underset{\mathbf{x}}{\min }F(\mathbf{x})\\ & =\arg \underset{\mathbf{x}}{\min }\sum _i{e}_i(\mathbf{x})\\ & =\arg \underset{\mathbf{x}}{\min }\sum _i{\mathbf{e}}_i(\mathbf{x}{)}^T{\mathbf{\Omega }}_i{\mathbf{e}}_i(\mathbf{x}).\end{array}$$

where ${\mathbf{\Omega }}_i$ encodes our uncertainty in each measurement.

A general solution would be to derive the global error function analytically and find its nulls — but in general this is complex with no closed-form solution.

↳ Use numerical approaches.

Assumptions

• A good initial guess is available.
• The error functions are "smooth" in the neighbourhood of the (hopefully) global minima.

↳ Then we can solve by iterative local linearization.

Solve via iterative local linearization

1. Linearize the error terms around the current solution.
2. Compute the first derivative of the squared error function.
3. Set it to zero and solve the resulting linear system.
4. Obtain the new state (hopefully closer to the minimum).
5. Iterate.

Linearize the error function

Approximate the error functions around an initial guess $\mathbf{x}$ via a Taylor expansion:

$${\mathbf{e}}_i(\mathbf{x}+\Delta \mathbf{x})\approx {\mathbf{e}}_i+{\mathbf{J}}_i(\mathbf{x})\Delta \mathbf{x}$$

where ${\mathbf{J}}_i$ is the Jacobian of ${\mathbf{e}}_i$ w.r.t. $\mathbf{x}$.

Squared error

With the linearization, fix $\mathbf{x}$ and minimize in the increments $\Delta \mathbf{x}$. Substituting:

$$\begin{array}{rl}{e}_i(\mathbf{x}+\Delta \mathbf{x})& \approx ({\mathbf{e}}_i+{\mathbf{J}}_i\Delta \mathbf{x}{)}^T{\mathbf{\Omega }}_i({\mathbf{e}}_i+{\mathbf{J}}_i\Delta \mathbf{x})\\ & =\underset{c_i}{\underbrace{{\mathbf{e}}_i^T{\mathbf{\Omega }}_i{\mathbf{e}}_i}}+2\underset{{\mathbf{b}}_i^T}{\underbrace{{\mathbf{e}}_i^T{\mathbf{\Omega }}_i{\mathbf{J}}_i}}\Delta \mathbf{x}+\Delta {\mathbf{x}}^T\underset{{\mathbf{H}}_i}{\underbrace{{\mathbf{J}}_i^T{\mathbf{\Omega }}_i{\mathbf{J}}_i}}\Delta \mathbf{x}\end{array}$$

So

$$e_i(\mathbf{x}+\Delta \mathbf{x})\approx c_i+2{\mathbf{b}}_i^T\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{H}}_i\Delta \mathbf{x}$$

Global error

The global error is the sum of squared error terms — and the approximation in the neighbourhood of the current $\mathbf{x}$ is:

$$\begin{array}{rl}F(\mathbf{x}+\Delta \mathbf{x})& \approx \sum _i(c_i+2{\mathbf{b}}_i^T\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{H}}_i\Delta \mathbf{x})\\ & =c+2{\mathbf{b}}^T\Delta \mathbf{x}+\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}\end{array}$$

with

$${\mathbf{b}}^T=\sum _i{\mathbf{e}}_i^T{\mathbf{\Omega }}_i{\mathbf{J}}_i,\mathbf{H}=\sum _i{\mathbf{J}}_i^T{\mathbf{\Omega }}_i{\mathbf{J}}_i.$$

Quadratic-form minimization

The global error is now a quadratic form in $\Delta \mathbf{x}$:

$$F(\mathbf{x}+\Delta \mathbf{x})\approx c+2{\mathbf{b}}^T\Delta \mathbf{x}+\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}$$

The approximate derivative w.r.t. $\Delta \mathbf{x}$ is

$$\frac{\partial F(\mathbf{x}+\Delta \mathbf{x})}{\partial \Delta \mathbf{x}}\approx 2\mathbf{b}+2\mathbf{H}\Delta \mathbf{x}.$$

Setting it to zero (minimum condition):

$$\mathbf{H}\Delta \mathbf{x}=-\mathbf{b}\Rightarrow \Delta {\mathbf{x}}^{\star }=-{\mathbf{H}}^{-1}\mathbf{b}$$

Gauss-Newton solution

algorithmGauss_Newton(x)

1. eᵢ(x + Δx) ≈ eᵢ(x) + Jᵢ Δx# Linearize
2. b = Σ eᵢᵀ Ωᵢ Jᵢ , H = Σ Jᵢᵀ Ωᵢ Jᵢ# Build linear system
3. H Δx⋆ = −b ⇒ Δx⋆ = −H⁻¹ b# Solve
4. x ← x + Δx⋆# Update state
5. iterate# Repeat until convergence

VisualizationGauss-Newton — fitting y = a + b·x

⏸ Pause↺ Reset🎲 Reseed data

Iter0

Init a-2.00

Init b-1.00

Noise σ0.30

The viz walks through Gauss-Newton on a tiny example: fit $y=a+bx$ to noisy data. From a bad initial guess (the early light-coloured lines), each iteration computes $\mathbf{H}\Delta \mathbf{x}=-\mathbf{b}$, updates $(a,b)$, and lands on the next line. After a handful of iterations the current line (solid) settles near the dashed ground truth.

Gauss-Newton summary

• Method to minimize a sum of squared errors.
• Start with an initial guess.
• Linearize the individual error functions.
• This yields a quadratic form.
• Obtain a linear system by setting its derivative to zero.
• Solving the linear system gives a state update.
• Iterate.

---

Relation to probabilistic state estimation

So far, we've minimized an error function. How does this relate to state estimation in the probabilistic sense?

General state estimation

Using Bayes' rule, independence, and the Markov assumption:

$$p(x_{0:t}\mid z_{1:t},u_{1:t})\propto p(x_0)\prod _{t}p(x_t\mid x_{t-1},u_t)p(z_t\mid x_t).$$

Taking the log-likelihood:

$$\log p(x_{0:t}\mid z_{1:t},u_{1:t})=\text{const.}+\log p(x_0)+\sum _t[\log p(x_t\mid x_{t-1},u_t)+\log p(z_t\mid x_t)].$$

For a Gaussian $\mathcal{N}(x;\mu ,\Sigma )$:

$$\log \mathcal{N}(x;\mu ,\Sigma )=\text{const.}-\frac{1}{2}\underset{{\mathbf{e}}^T(x)}{\underbrace{(x-\mu {)}^T}}\underset{\mathbf{\Omega }}{\underbrace{{\Sigma }^{-1}}}\underset{\mathbf{e}(x)}{\underbrace{(x-\mu )}}$$

Up to a constant, the log-likelihood is the same as the error functions we used before:

$$\log p(x_{0:t}\mid z_{1:t},u_{1:t})=\text{const.}-\frac{1}{2}{e}_p(x)-\frac{1}{2}\sum _t[e_{u_t}(x)+e_{z_t}(x)]$$

where $e_p$ is the prior, $e_{u_t}$ is the motion (odometry) error, and $e_{z_t}$ is the measurement error.

Maximizing the log-likelihood is equivalent to:

$$\arg \underset{x}{\max }\log p(x_{0:t}\mid z_{1:t},u_{1:t})\equiv \arg \underset{x}{\min }(e_p(x)+\sum _t[e_{u_t}(x)+e_{z_t}(x)]).$$

Takeaway:

• Least squares (with Gaussian assumptions) is equivalent to Maximum A Posteriori (MAP) estimation.
• Minimizing the sum of weighted squared residuals corresponds to maximizing the posterior probability.

Summary

• Technique to minimize squared error functions.
• Gauss-Newton is an iterative approach for non-linear problems.
• Uses linearization (approximation).
• Equivalent to maximizing the log-likelihood of independent Gaussians.
• Popular method in many disciplines.