Nonlinear least squares

Almost every batch estimation problem in robotics is

$$\mathbf{x}^\star = \arg\min_{\mathbf{x}} \sum_i \lVert \mathbf{r}_i(\mathbf{x}) \rVert^2_{\Sigma_i},$$

a sum of squared residuals weighted by their covariances. Maximum likelihood under Gaussian noise gives exactly this, which is where the squares come from — and also where their fragility comes from, since a Gaussian assigns vanishing probability to large errors and therefore an enormous cost to explaining one.

Linearise $\mathbf{r}(\mathbf{x} + \Delta) \approx \mathbf{r} + J\Delta$ and the minimiser of the linearised problem solves the normal equations

$$\left(J^\top \Sigma^{-1} J\right)\Delta = -J^\top \Sigma^{-1}\mathbf{r}.$$

The matrix on the left is the information matrix of the estimate. The same object as in the filter, arrived at from the optimisation side.

Robust kernels address a different failure. One wrong data association produces a residual of a hundred pixels; squared, it dominates the sum, and the optimiser distorts the entire solution to reduce it. A robust kernel $\rho(\cdot)$ grows sub-quadratically — Huber is quadratic near zero and linear beyond a threshold $\delta$ — so a large residual contributes a bounded gradient.

The implementation is one line in an IRLS loop: multiply each residual's information by $w_i = \rho'(\lVert r_i\rVert)/\lVert r_i \rVert$ and re-solve. For Huber, $w_i = 1$ inside the band and $\delta/\lVert r_i \rVert$ outside. So a robust kernel is literally down-weighting suspicious measurements, and $\delta$ is the statement "residuals beyond this are probably not noise".