Skip to the lesson
FixTheRobot Explore Robotics / Localization & SLAM / M4 Graph-Based SLAM / M4-T3

Localization & SLAM · Theory · M4

M4-T3 — Nonlinear Least Squares

Linearise, solve a linear least-squares problem, step, and repeat.

  • Lesson 3 of 8
  • Module 4: Graph-Based SLAM
  • Theory
  • About 8 minutes to read

Every graph-based SLAM system, and every bundle adjuster, is a solver for the same problem: find the parameters that make a long list of residuals as small as possible. The algorithm that does it, Gauss-Newton and its damped cousin Levenberg-Marquardt, is a few lines of linear algebra built on one idea: pretend the problem is linear near where you are. This lesson derives it and runs it by hand on a small example.

Learning objectives

After this lesson, you should be able to:

  • State a nonlinear least-squares problem and its weighted form.
  • Derive the Gauss-Newton step from the first-order expansion of the residuals.
  • Explain how Levenberg-Marquardt damping makes the step safe, and how it is adapted.
  • Describe convergence and failure: local minima, poor initial guesses, ill-conditioned problems.

The problem and the intuition

You are blindfolded on a hillside and want to reach the lowest point. At each step you feel the local slope and curvature and jump to the bottom of the best-fitting bowl. If the hill is bowl-shaped, you are done in a step or two. If the bowl is a poor description of the terrain you may leap past the valley, so a careful walker takes a smaller step when the last jump made things worse. Gauss-Newton is the bold jumper; Levenberg-Marquardt is the one who shortens the stride when the jump fails.

Nonlinear least squares is solved by repeated linear least squares. Each iteration replaces the residuals by their straight-line approximation, solves exactly, and steps.

Gauss-Newton iterations on a range-only fixThree beacons and a robot position estimate. The estimate starts at (3,2) and moves in three steps to (1.46, 1.00), close to the true position (1.5, 1.0). The cost falls from 3.90 to 0.0088. beacon (0,0)beacon (4,0)beacon (2,3) start (3, 2)step 1: cost 3.900step 2: cost 0.238step 3: cost 0.0094step 4: cost 0.0088 (converged) Green ring: true position (1.5, 1.0).
Four Gauss-Newton iterations locating a robot from three range measurements. The cost drops by an order of magnitude per step at first, then settles at the noise level.

The algorithm: Gauss-Newton and Levenberg-Marquardt

Problem. Given residual functions rk(x) (each a vector, such as the error of one edge), find the parameters x that minimise the weighted sum of squares

Weighted nonlinear least squares
F(x)=∑k rk(x)⊤ Ωk rk(x)=r(x)⊤ Ω r(x)F(x)=\sum _{k}\,r_{k}(x)^{\top}\,\Omega _{k}\,r_{k}(x)=r(x)^{\top}\,\Omega \,r(x)

Assumptions. The residuals are differentiable and not too nonlinear near the solution; the errors are Gaussian with covariance Ω⁻¹ (this makes the minimiser a maximum-likelihood estimate); a starting point x0 in the basin of the minimum is available.

Gauss-Newton

Expand each residual to first order around the current estimate x: r(x + Δ) ≈ r(x) + J Δ, where J = ∂r/∂x is the Jacobian. Substituting gives a quadratic in Δ that is minimised exactly by the normal equations:

Gauss-Newton step
H Δ=−bH\,\Delta =-b
H=J⊤ Ω JH=J^{\top}\,\Omega \,J
b=J⊤ Ω rb=J^{\top}\,\Omega \,r
x←x+Δx\leftarrow x+\Delta
J
Jacobian of the stacked residuals: row k holds the derivatives of rk with respect to every parameter. For a pose graph it is extremely sparse.
H
the (Gauss-Newton) approximation of the Hessian of F: Jᵀ Ω J. Symmetric and positive semi-definite.
b
the gradient of F/2: Jᵀ Ω r. The step moves downhill.
Δ
the update. For rotations it is a small rotation applied on the manifold, not an added number.

The Gauss-Newton approximation drops the term with second derivatives of r, which is small when the residuals are small near the solution (a good fit). It makes each step cheap and the step positive-definite.

Levenberg-Marquardt

When the starting point is far or the problem is poorly conditioned, the linearisation is wrong over a full step and F may increase. Levenberg-Marquardt damps the normal equations:

Levenberg-Marquardt step
(H+λ D) Δ=−b(H+\lambda \,D)\,\Delta =-b
D=I  orD=I\;\mathrm{or}
diag⁡(H)\operatorname{diag} (H)

A large λ makes the step short and along the gradient (safe, slow); a small λ makes it a Gauss-Newton step (fast near the minimum). The standard rule: compute the gain ratio ρ = (actual decrease of F) / (decrease predicted by the linear model). If ρ is positive and not tiny, accept the step and reduce λ (for example ÷ 3); otherwise reject it and increase λ (× 2 or × 10) and try again.

Strengths and failure modes

PropertyGauss-NewtonLevenberg-Marquardt
Convergence near the solutionQuadratic when residuals are smallSame, as λ → 0
Far from the solutionCan overshoot or divergeSafe: never increases F (it rejects bad steps)
Singular or ill-conditioned HFails or gives huge stepsλ regularises it
Local minimaFinds the nearest oneSame: neither is global
Cost per iterationOne sparse linear solveThe same, plus retries

Neither finds the global minimum. SLAM graphs with large rotation errors can have several, which is why initialisation matters (M4-T4).

A worked example: a position fix from three ranges

A robot measures its distance to three beacons at B1 = (0, 0), B2 = (4, 0), B3 = (2, 3), getting z = (1.84, 2.77, 2.12) m. The unknown is x = (x, y). The residuals are rk = |x − Bk| − zk with the Jacobian rows (x − Bk)ᵀ / |x − Bk|. Take Ω = I and start at x0 = (3, 2).

  1. Iteration 1. The distances at (3, 2) are 3.61, 2.24, 1.41, so r = (1.77, −0.53, −0.71) and the cost F = 3.90. The Jacobian rows are (0.83, 0.55), (−0.45, 0.89), (0.71, −0.71). Solving JᵀJ Δ = −Jᵀ r gives Δ = (−1.164, −0.940): x1 = (1.836, 1.060). The cost drops to 0.238.
  2. Iteration 2. From x1 the step is Δ = (−0.365, −0.076): x2 = (1.471, 0.984), cost 0.0094. A 25-fold reduction.
  3. Iteration 3. Δ = (−0.006, +0.020): x3 = (1.4645, 1.0038), cost 0.00885.
  4. Iteration 4. Δ ≈ (−0.0006, 0.0008): converged. The remaining cost 0.0088 is the measurement noise: the true position was (1.5, 1.0) and the answer, (1.464, 1.005), is 3.6 cm off because the three ranges carry a few centimetres of noise.
  5. From a bad start. From (−3, −2), plain Gauss-Newton gets to (0.29, −0.09), then (1.68, 0.57), (1.49, 0.99) and converges in five steps (cost 47.97 → 5.23 → 0.26 → 0.010). On this well-behaved problem even a bad start works. On a pose graph with 90° of accumulated rotation error the first step could throw nodes to opposite sides of a loop; there, damping saves the run.
  6. Levenberg-Marquardt on the same problem with λ starting at 1: costs 3.90 → 1.67 → 0.070 → 0.0092 → 0.00885, with λ decreasing 1 → 0.33 → 0.11 → 0.04. It takes one more iteration but each step is guaranteed to reduce the cost.

Two ingredients carried the solution: the Jacobian and the sparsity of H. For a SLAM graph the same computation is done with many more parameters but the same steps.

Engineering implications and limitations

Engineering implications

  • Check Jacobians numerically. A wrong Jacobian still often converges, slowly, to the wrong answer. Compare with finite differences, as in M2-T6.
  • Use a sparse solver. H is sparse; a sparse Cholesky factorisation (CHOLMOD, Eigen’s SimplicialLDLT, Ceres, GTSAM) solves 10⁵ unknowns in seconds. Dense solves do not scale.
  • Always use LM (or a trust region) for real data. Plain Gauss-Newton is for teaching and well-initialised problems. Ceres’s default is Levenberg-Marquardt; g2o offers both.
  • Stop sensibly. Stop when the relative decrease of F, the step norm, or the gradient norm is below a tolerance (for example 1e-6), or after a maximum number of iterations. Report the final cost and the number of iterations: a rising iteration count signals a poor initial guess.
  • Scale the problem. Parameters of very different magnitudes (metres and radians, pixels and metres) make H ill-conditioned. Use sensible units and information weights.
  • Update rotations on the manifold: x ⊞ Δ = x · exp(Δ), not x + Δ (M2-T2).

Limits

Both methods are local. A bad starting point, repeated structure (symmetric buildings) or a wrong constraint can leave them in a wrong minimum with a low-looking cost. Gauss-Newton also assumes Gaussian errors: a single outlier with a big residual dominates the sum of squares, which is the motivation for robust costs (M4-T6). Global methods (convex relaxations, certifiable solvers such as SE-Sync) exist but are research tools (Module 8).

Common misconceptions

“Gauss-Newton is gradient descent.”

Gradient descent steps along −b with a fixed step length. Gauss-Newton also uses H to scale and rotate the step to the local curvature: it is a Newton-like method and converges much faster.

“If the cost decreases, the answer is right.”

The cost decreases to a local minimum. A low cost with a wrong structure is possible.

“Damping slows convergence at the end.”

LM reduces λ as it converges, so near the solution it behaves like Gauss-Newton.

“The Jacobian is a nuisance computed by the library.”

Its correctness decides everything; errors there are the commonest cause of mysterious non-convergence.

Engineering takeaways

  1. Nonlinear least squares minimises Σ rᵀΩr; it is solved iteratively by linearising the residuals.
  2. Gauss-Newton solves JᵀΩJ Δ = −JᵀΩr each step: quadratic convergence near the solution.
  3. Levenberg-Marquardt adds λD to make steps safe and adapts λ by the gain ratio.
  4. Both find local minima: initialisation and outlier handling matter.
  5. Verify Jacobians, use sparse solvers, and update rotations on the manifold.

References and further reading

Primary papers and authoritative documentation.

  • Nocedal, J. and Wright, S. J. (2006). Numerical Optimization, 2nd ed., Springer, chapter 10: least-squares problems.
  • Madsen, K., Nielsen, H. B. and Tingleff, O. (2004). Methods for Non-Linear Least Squares Problems, 2nd ed. Technical University of Denmark.
  • Levenberg, K. (1944). A method for the solution of certain non-linear problems in least squares. Quarterly of Applied Mathematics 2(2). Marquardt, D. W. (1963). An algorithm for least-squares estimation of nonlinear parameters. SIAM Journal 11(2).
  • Triggs, B., McLauchlan, P., Hartley, R. and Fitzgibbon, A. (2000). Bundle adjustment: a modern synthesis. Vision Algorithms: Theory and Practice.
  • Agarwal, S., Mierle, K. et al. Ceres Solver, documentation: Non-linear least squares (ceres-solver.org).

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

Why does Gauss-Newton usually converge very quickly near the solution when the residuals are small?

Explanation

It ignores the second-derivative term of the true Hessian. When the residuals are small near the solution that term is small, so JᵀΩJ is almost the exact Hessian and the step is almost a Newton step, which converges quadratically.

Question 2

In Levenberg-Marquardt a step increases the cost. What should the algorithm do, and why does it help?

Explanation

Reject the step and increase λ, then recompute. A larger λ makes the step shorter and more like gradient descent, where the linear model is more trustworthy, so a smaller step can be guaranteed to reduce the cost.

Question 3

A pose-graph optimisation converges to a low cost, but the map has a corridor folded back on itself. What is a likely cause and how would you test it?

Explanation

A local minimum caused by a poor initial guess (large accumulated rotation error) or a wrong loop closure. Test by re-running from a better initialisation (for example rotation averaging first), and by checking the residuals of individual edges for one with an abnormally large error.

What’s next

Applying Gauss-Newton to the pose graph needs one more piece: the Jacobians of the SE(2) edge error, their sparse structure, and the anchor.

Practice this lesson

Correct Accumulated Trajectory Drift

Implement Gauss-Newton on a pose graph and bring a drifted loop trajectory back together.

Next lesson

M4-T4 — Graph Optimization

Apply Gauss-Newton to SE(2) pose graphs, with the sparse structure, gauge freedom and an anchor.

Nothing below is required to finish this lesson. The last 3 entries are lessons of other courses on this site that teach the background or go further.