The algorithm that turns a pile of noisy range and pixel measurements into the single best estimate of where something actually is: GPS receivers, self-driving cars, robot SLAM, and the software that stitches your phone's panoramas together all lean on it.
You are a small robot parked somewhere in a rectangular room. Three beacons are bolted to the walls at known, fixed positions. Each beacon replies to a ping with the time it took to hear you, which your receiver converts into a distance: beacon one says you are about 5.2 meters away, beacon two says 8.0 meters, beacon three says 6.9 meters. Where are you?
If those three distances were exact, this would be pure geometry. Draw a circle of radius 5.2 around beacon one, a circle of radius 8.0 around beacon two, a circle of radius 6.9 around beacon three. All three circles should cross at exactly one point: you. This trick, called trilateration, is the same idea GPS uses with satellites instead of wall beacons.
But real distances are never exact. Sound bounces off the walls before it arrives (multipath), the beacon's clock drifts by a microsecond, a draft nudges the reading. So the three circles do not meet at one clean point. They form a small triangle of near misses. Somewhere inside or near that triangle is your true position, and your job is to find the single point that best explains all three noisy readings at once.
Here is the part that makes this hard: the equation connecting your unknown position (x, y) to a measured range is a square root of a sum of squares, range = sqrt((x - bx)^2 + (y - by)^2). That square root is a curve, not a straight line. You cannot just add up three linear equations and solve for two unknowns the way you did in algebra class. The relationship between what you want (position) and what you measured (range) is nonlinear: doubling your distance from a beacon does not double any coordinate in any simple way.
Three beacons (teal squares) each report a noisy range to an unknown robot. Drag the orange dot to guess the robot's position. The three thin circles show what each beacon's measured range implies. The number at the top is the total squared error, the sum of how far your guess is from matching all three circles at once. Try to make it as small as you can before reading on.
Notice that no single drag point makes every circle line up perfectly. That is the signature of noisy, over-determined measurements: three readings, two unknowns, and no guarantee of a perfect fit. The best you can do is minimize the total squared miss. That minimization, done by hand once and then automated forever, is the entire subject of this lesson.
Before tackling curved relationships, let's master the straight ones. Suppose you are calibrating a cheap sensor. At three known reference temperatures you record a voltage: at 0 degrees you read 1.0 volts, at 1 degree you read 2.0 volts, at 2 degrees you read 2.0 volts. You expect a linear response, voltage equals a + b * temperature, and you want the single best line through these three noisy points.
Three points, two unknowns (the intercept a and the slope b): that is one more equation than you need, which is exactly the situation from Chapter 0, except this time the relationship is a straight line instead of a circle. Write the three equations out:
Stack the unknowns into a vector and the equations into a matrix, and this becomes A·x = b, where A is 3 rows by 2 columns, x = [a, b], and b is the three voltage readings:
A tall skinny matrix like this, more rows than columns, describes an over-determined system: more equations than unknowns. In general no exact x exists that satisfies every row. So instead we ask for the x that minimizes the total squared gap, the sum of (A·x - b)^2 across every row. That minimizer has a name: the least squares solution, and it satisfies a tidy set of equations called the normal equations:
Where does that come from? Picture every possible line as a point in an abstract space, and picture the vector of predictions A·x as always living on a flat plane (called the column space of A) no matter what x you pick, because A·x is just some combination of A's two columns. The measured vector b sits somewhere off that plane, since no line fits exactly. The closest point on the plane to b is the one where the leftover error, the residual r = b - A·x, points straight out of the plane at a right angle: it is perpendicular to every column of A. "Perpendicular to every column of A" is written algebraically as AT·r = 0, and substituting r = b - A·x gives exactly AT·A·x = AT·b. This is the projection picture: least squares finds the shadow that b casts onto the plane of everything A can produce, and that shadow is the closest reachable point.
Let's grind through the actual numbers. First compute AT·A, a 2×2 matrix. Its entries are dot products of the columns of A. Column one is [1, 1, 1], column two is [0, 1, 2].
Check each entry by hand: top left is column one dotted with itself, 1·1 + 1·1 + 1·1 = 3. Top right and bottom left are column one dotted with column two, 1·0 + 1·1 + 1·2 = 0 + 1 + 2 = 3. Bottom right is column two dotted with itself, 0·0 + 1·1 + 2·2 = 0 + 1 + 4 = 5. Next compute AT·b, a 2×1 vector: the first entry sums b directly (since column one of A is all ones), 1 + 2 + 2 = 5; the second entry weights each reading by its temperature, 0·1 + 1·2 + 2·2 = 0 + 2 + 4 = 6.
Now solve the 2×2 system 3a + 3b = 5 and 3a + 5b = 6 by elimination. Subtract the first equation from the second: (3a + 5b) - (3a + 3b) = 6 - 5, which simplifies to 2b = 1, so b = 0.5. Plug that back into the first equation: 3a + 3(0.5) = 5, so 3a = 3.5, giving a = 7/6 ≈ 1.1667. The best fit line is voltage = 1.1667 + 0.5·temperature.
Plug each temperature back in and compare to the measured voltage to find the residuals: at temperature 0, predicted 1.1667, measured 1, residual -0.1667. At temperature 1, predicted 1.6667, measured 2, residual +0.3333. At temperature 2, predicted 2.1667, measured 2, residual -0.1667. Add the three residuals: -0.1667 + 0.3333 - 0.1667 = 0. That is not a coincidence: whenever a model includes a plain intercept term, the least squares residuals always sum to exactly zero, because that is precisely what the first normal equation, 3a + 3b = 5, is enforcing.
Now confirm the by-hand answer with three lines of NumPy, using the exact same A and b from above:
python import numpy as np A = np.array([[1, 0], [1, 1], [1, 2]], dtype=float) b = np.array([1.0, 2.0, 2.0]) # normal equations, spelled out AtA = A.T @ A # [[3, 3], [3, 5]] Atb = A.T @ b # [5, 6] x = np.linalg.solve(AtA, Atb) # [1.1667, 0.5] matches the hand solve # or skip the normal equations entirely: lstsq does the same thing internally x2, *_ = np.linalg.lstsq(A, b, rcond=None) print(x, x2) # [1.16666667 0.5] [1.16666667 0.5]
1.16667, slope 0.5. np.linalg.solve follows the normal-equations path exactly as derived above; np.linalg.lstsq uses a more numerically careful factorization internally (it avoids explicitly forming ATA, which can lose precision on ill-conditioned data) but is solving the identical minimization problem and lands on the identical answer here.Drag the slope slider and watch the fitted line rotate. The vertical dashed segments are the residuals. Watch how the total squared length of those segments (shown at top) hits its minimum exactly at the least squares slope you just computed, 0.5, and how the sum of the (signed) residuals stays pinned at zero the moment the intercept is also least squares optimal.
| Symbol | Shape | Meaning here |
|---|---|---|
| A | 3×2 | Design matrix: one row per data point, columns are [1, temperature] |
| x | 2×1 | Unknowns: [intercept, slope] |
| b | 3×1 | Measured voltages |
| r = b - Ax | 3×1 | Residuals: what the fitted line misses at each point |
| ATA | 2×2 | The normal-equations matrix, always square and symmetric |
Every method in this lesson chases the same quantity: the residual, the signed gap between what a model predicts and what was actually measured, r = prediction - measurement. Once you have residuals, you need to collapse them into one number that says "how good is this guess overall." The obvious first idea, just add up the residuals, fails immediately: a residual of +3 and a residual of -3 would cancel to zero, even though both are equally wrong. You need a measure that treats overshoot and undershoot as equally bad.
Two natural fixes exist: take the absolute value of each residual and add those up, or square each residual and add those up. Both fixes make errors non-negative. Why did the field settle on squares almost everywhere? The honest answer is not that squares are more "natural," it is that squares fall directly out of a specific, extremely common assumption about how the noise behaves.
Assume every measurement error is drawn from a bell-shaped (Gaussian) distribution centered on the truth, with the same spread for every measurement. The probability of seeing a particular set of measurements, given a candidate model, multiplies together one bell-curve density per measurement. Multiplying bell curves and then taking a logarithm (a standard trick because logarithms turn products into sums and are easier to maximize) leaves you with an expression that is, up to a constant, exactly the negative of a sum of squared residuals. Maximizing "how likely were these measurements under my model" therefore becomes identical to minimizing "the sum of squared residuals." That equivalence has a name: least squares is the maximum likelihood estimate under Gaussian noise.
A quick numerical check makes the equivalence concrete. Suppose one beacon's range noise really is Gaussian with a spread of 0.3 meters, and you are comparing two candidate positions: candidate A leaves a residual of 0.1 there, candidate B leaves a residual of 0.6. Plugging both into the Gaussian density formula gives candidate A a density of about 1.26 and candidate B a density of about 0.18, so candidate A is roughly seven times more likely than candidate B given that measurement. Now look at the squared residuals instead: 0.01 for candidate A, 0.36 for candidate B. Inside the Gaussian formula, the exponent is exactly the negative of the squared residual divided by twice the noise spread squared, so whichever candidate has the smaller squared residual always has the larger likelihood, and that relationship holds no matter which two candidates you compare or how many measurements you combine. That is the real content of the equivalence: minimizing the sum of squared residuals and maximizing the Gaussian likelihood always rank candidates in the same order, because one is a strictly decreasing function of the other.
To make the growth rate concrete, compare squared cost to absolute cost side by side. A residual of 1 costs 1 under both. A residual of 5 costs 25 under squared cost but only 5 under absolute cost, five times more punishing under squares. A residual of 10 costs 100 under squared cost but only 10 under absolute cost, ten times more punishing. The widget below plots both cost curves and lets you drag a residual value along the x-axis to watch the gap between them grow.
For our beacon problem, once we accept squared cost, the total objective we are minimizing over the unknown position x = (x, y) is:
Here hi(x) is the range you would predict from a candidate position to beacon i, and zi is the range actually measured. This J(x) is exactly the number the widget in Chapter 0 was computing live as you dragged the orange dot: it is the height of a curved bowl-shaped surface hovering over the floor of possible positions, and the whole rest of this lesson is a collection of clever ways to find the bottom of that bowl without checking every point on the floor.
Back to the curved beacon problem. Linear least squares from Chapter 1 solved everything in one shot because the model was a straight line. Our range model h(x) = sqrt((x-bx)^2 + (y-by)^2) is curved, so there is no one-shot formula. Gauss-Newton gets around this with a simple, repeatable trick: near any single point, even a curved function looks almost like a straight line if you zoom in far enough. So Gauss-Newton picks a starting guess, approximates the curved residuals with a straight (linear) model right at that guess, solves that linear problem exactly using the normal equations from Chapter 1, steps to the new answer, and repeats the whole process from there.
The linear approximation of the residual near a guess x uses the same first-order idea as a tangent line: h(x + Δx) ≈ h(x) + J·Δx, where J is the Jacobian, a matrix of partial derivatives (one row per measurement, one column per unknown) that tells you how fast each predicted range changes as you nudge x or y. For a single range measurement to a beacon at (bx, by), the two partial derivatives are:
That formula is worth pausing on: it says the sensitivity of the range to a small move in x is just the x-component of the unit vector pointing from the beacon toward your current guess. Moving directly away from a beacon changes its measured range at the fastest possible rate (sensitivity close to 1); moving sideways, perpendicular to the line toward the beacon, barely changes the range at all (sensitivity close to 0). This is exactly why a single range measurement pins you down along one direction (toward or away from the beacon) but tells you almost nothing about your position perpendicular to that direction, a fact Chapter 7 turns into a precise number.
Place three beacons at B1 = (0, 0), B2 = (10, 0), B3 = (0, 10). The true (unknown to our solver) robot position is (3, 4), a 3-4-5 triangle away from beacon one, giving a clean true range of exactly 5 to that beacon. Our noisy measurements are z1 = 5.2, z2 = 8.0, z3 = 6.9. We start the search from an initial guess of x0 = (5, 5), roughly the middle of the room.
Step 1, predict the ranges from the current guess. From (5, 5) to beacon one at the origin: sqrt(5^2 + 5^2) = sqrt(50) = 7.0711. By the symmetry of this particular layout, the distance from (5, 5) to beacon two at (10, 0) is sqrt((5-10)^2 + (5-0)^2) = sqrt(25 + 25) = sqrt(50) = 7.0711, and the distance to beacon three at (0, 10) is sqrt(5^2 + (5-10)^2) = sqrt(25+25) = 7.0711 as well. All three predicted ranges happen to be identical at this particular starting guess.
Step 2, compute the residuals. Residual equals predicted range minus measured range: r1 = 7.0711 - 5.2 = 1.8711, r2 = 7.0711 - 8.0 = -0.9289, r3 = 7.0711 - 6.9 = 0.1711. The current cost is J(x0) = 1.8711^2 + (-0.9289)^2 + 0.1711^2 = 3.5010 + 0.8629 + 0.0293 = 4.393.
Step 3, build the Jacobian. Each row uses the unit-vector formula above, from the guess (5,5) toward each beacon. For beacon one at (0,0): ((5-0)/7.0711, (5-0)/7.0711) = (0.7071, 0.7071). For beacon two at (10,0): ((5-10)/7.0711, (5-0)/7.0711) = (-0.7071, 0.7071). For beacon three at (0,10): ((5-0)/7.0711, (5-10)/7.0711) = (0.7071, -0.7071).
Step 4, form and solve the normal equations for the step. Gauss-Newton solves JTJ · Δx = -JTr, the same normal-equations machinery from Chapter 1, just rebuilt fresh at every iteration. Computing JTJ entry by entry: the top-left entry is column one of J dotted with itself, 0.7071^2 + (-0.7071)^2 + 0.7071^2 = 0.5+0.5+0.5 = 1.5. The bottom-right entry, column two dotted with itself, is also 1.5 by the same symmetry. The off-diagonal entry, column one dotted with column two, is 0.7071·0.7071 + (-0.7071)·0.7071 + 0.7071·(-0.7071) = 0.5 - 0.5 - 0.5 = -0.5.
Next, JTr: the first entry is column one of J dotted with r, 0.7071(1.8711) + (-0.7071)(-0.9289) + 0.7071(0.1711) = 0.7071 × (1.8711+0.9289+0.1711) = 0.7071 × 2.9711 = 2.1013. The second entry is column two dotted with r, 0.7071(1.8711) + 0.7071(-0.9289) + (-0.7071)(0.1711) = 0.7071 × (1.8711-0.9289-0.1711) = 0.7071 × 0.7711 = 0.5453.
Now solve the 2×2 system [[1.5,-0.5],[-0.5,1.5]] · Δx = [-2.1013, -0.5453]. The determinant of the matrix is 1.5×1.5 - (-0.5)×(-0.5) = 2.25 - 0.25 = 2.0, so the inverse is (1/2) · [[1.5, 0.5],[0.5, 1.5]]. Multiplying that inverse by the right-hand side: Δx = 0.5×(1.5×(-2.1013) + 0.5×(-0.5453)) = 0.5×(-3.1520 - 0.2727) = 0.5×(-3.4247) = -1.7123. And Δy = 0.5×(0.5×(-2.1013) + 1.5×(-0.5453)) = 0.5×(-1.0507 - 0.8180) = 0.5×(-1.8687) = -0.9343.
Step 5, take the step. The new position after one iteration is x1 = x0 + Δx = (5 - 1.7123, 5 - 0.9343) = (3.2877, 4.0657). Compare that to the true position, (3, 4): after a single Gauss-Newton iteration starting from a fairly rough guess, we landed within about a third of a meter, in both coordinates, of the truth.
Does another iteration help? Repeat exactly the same five steps starting from x1 = (3.2877, 4.0657) instead of x0: predict the three ranges from x1, subtract the same measurements to get new residuals, rebuild the Jacobian from the new unit vectors, solve the new 2×2 normal equations, and step again. Carrying the arithmetic through (same method, new numbers) gives a second step of Δx ≈ (-0.0807, -0.0458), landing at x2 = (3.2070, 4.0199).
| Iteration | Position | Cost J(x) | Distance to true (3,4) |
|---|---|---|---|
| 0 (start) | (5.0000, 5.0000) | 4.393 | 2.236 |
| 1 | (3.2877, 4.0657) | 0.0375 | 0.295 |
| 2 | (3.2070, 4.0199) | 0.0279 | 0.208 |
Two things are worth noticing here. First, the jump from iteration 0 to iteration 1 did almost all of the work, shrinking the distance to the truth by nearly eightfold in a single step; the jump from 1 to 2 was a much smaller refinement. This front-loaded convergence is typical of Gauss-Newton: the first step, however rough, usually gets you into the "nearly linear" neighborhood of the true optimum, and every step after that is cleaning up a shrinking remainder. Second, notice that the cost never reaches exactly zero, and the position never lands exactly on (3, 4). That is not a bug. The measured ranges, 5.2, 8.0, and 6.9, are themselves noisy versions of the true ranges 5.0, 8.0623, and 6.7082, so the point that best explains those three noisy numbers is not identical to the true position that generated them. Gauss-Newton is converging correctly, to the honest best-fit answer given noisy data, which is the whole point of Chapter 2.
Here is that exact Gauss-Newton step (Jacobian, normal equations, solve) written first with no libraries at all, then with NumPy's built-in least squares solver, then with PyTorch. All three use the identical numbers from the hand computation above and should land on the identical step.
python (plain, no libraries) import math # beacons and noisy measured ranges beacons = [(0,0), (10,0), (0,10)] z = [5.2, 8.0, 6.9] x, y = 5.0, 5.0 # initial guess # --- predicted ranges and residuals --- h = [math.sqrt((x-bx)**2 + (y-by)**2) for bx,by in beacons] r = [h[i] - z[i] for i in range(3)] # --- Jacobian rows: unit vector from each beacon to the guess --- J = [[(x-bx)/h[i], (y-by)/h[i]] for i,(bx,by) in enumerate(beacons)] # --- normal equations JtJ, Jtr, built by hand (2x2 system) --- JtJ00 = sum(J[i][0]*J[i][0] for i in range(3)) JtJ01 = sum(J[i][0]*J[i][1] for i in range(3)) JtJ11 = sum(J[i][1]*J[i][1] for i in range(3)) Jtr0 = -sum(J[i][0]*r[i] for i in range(3)) Jtr1 = -sum(J[i][1]*r[i] for i in range(3)) # --- solve the 2x2 system by hand (Cramer's rule, zero-safe) --- det = JtJ00*JtJ11 - JtJ01*JtJ01 if abs(det) < 1e-12: dx, dy = 0.0, 0.0 else: dx = (JtJ11*Jtr0 - JtJ01*Jtr1) / det dy = (JtJ00*Jtr1 - JtJ01*Jtr0) / det print(f"step = ({dx:.4f}, {dy:.4f})") # step = (-1.7123, -0.9343)
python (numpy, lstsq) import numpy as np beacons = np.array([[0,0], [10,0], [0,10]], dtype=float) z = np.array([5.2, 8.0, 6.9]) guess = np.array([5.0, 5.0]) diff = guess - beacons # [3,2]: guess minus each beacon h = np.linalg.norm(diff, axis=1) # [3]: predicted ranges r = h - z # [3]: residuals J = diff / h[:, None] # [3,2]: unit vectors, Jacobian # lstsq solves J @ dx = -r in the least squares sense directly dx, *_ = np.linalg.lstsq(J, -r, rcond=None) print(f"step = ({dx[0]:.4f}, {dx[1]:.4f})") # step = (-1.7123, -0.9343)
python (pytorch, autograd Jacobian) import torch beacons = torch.tensor([[0.,0.], [10.,0.], [0.,10.]]) z = torch.tensor([5.2, 8.0, 6.9]) guess = torch.tensor([5.0, 5.0], requires_grad=True) def residuals(pos): return torch.norm(pos - beacons, dim=1) - z # [3] # autograd gives us the exact Jacobian: no hand differentiation needed J = torch.autograd.functional.jacobian(residuals, guess) # [3,2] r = residuals(guess).detach() # solve the normal equations J^T J dx = -J^T r JtJ = J.T @ J Jtr = J.T @ r dx = torch.linalg.solve(JtJ, -Jtr) print(f"step = ({dx[0]:.4f}, {dx[1]:.4f})") # step = (-1.7123, -0.9343)
(-1.7123, -0.9343), because they are the same linear algebra written three ways. The plain Python version shows every arithmetic operation your calculator would do; NumPy's lstsq hides the normal equations behind one battle-tested call; PyTorch's autograd.functional.jacobian even computes the derivatives for you, which becomes essential once your residual function is too complicated to differentiate by hand, exactly the situation in Chapter 5's bundle adjustment.Gauss-Newton is fast when it works, but it can misbehave when the starting guess is far from the truth or the local linear approximation is a poor match to the real curved cost surface. In those regimes the raw Gauss-Newton step can overshoot wildly, sometimes making the cost worse instead of better. Levenberg-Marquardt (LM) fixes this with one small, brilliant change: add a damping term to the normal equations before solving.
Here λ (lambda) is a single non-negative number, the damping parameter, and I is the identity matrix. Watch what happens at its two extremes. When λ = 0, this is exactly the Gauss-Newton equation from Chapter 3, unmodified. As λ grows very large, the λI term swamps JTJ, and the equation approaches λ·Δx ≈ -JTr, meaning Δx becomes a tiny step in the direction of -JTr, which is precisely the direction of steepest descent, the same direction plain gradient descent would take. Levenberg-Marquardt is therefore a single dial that slides continuously between "trust the local linear model completely" (Gauss-Newton, fast but sometimes reckless) and "take a small, cautious step downhill" (gradient descent, slow but safe).
Reusing the exact JTJ = [[1.5,-0.5],[-0.5,1.5]] and JTr = [2.1013, 0.5453] from Chapter 3's worked iteration, add damping λ = 5 to the diagonal: JTJ + 5I = [[6.5,-0.5],[-0.5,6.5]]. The determinant is 6.5×6.5 - 0.25 = 42.0, so solving the same way as before gives Δx = (1/42)(6.5×(-2.1013)+0.5×(-0.5453)) = (1/42)(-13.931) = -0.3317 and Δy = (1/42)(0.5×(-2.1013)+6.5×(-0.5453)) = (1/42)(-4.5951) = -0.1094. Compare: the undamped Gauss-Newton step was (-1.7123, -0.9343), a bold leap of length about 1.95. The λ=5 damped step is only (-0.3317, -0.1094), length about 0.35, roughly a sixth as long, and rotated to point more directly along the raw gradient direction. Damping did not change where downhill is, it changed how far you dare to walk before re-checking. Push λ to 20 with the same numbers and the same arithmetic gives Δx = (-0.0984, -0.0277), length about 0.10, smaller again.
| λ | Step Δx | Step length |
|---|---|---|
| 0 (Gauss-Newton) | (-1.7123, -0.9343) | 1.951 |
| 5 | (-0.3317, -0.1094) | 0.349 |
| 20 | (-0.0984, -0.0277) | 0.102 |
Every row solves the identical two-by-two system, only the diagonal padding changes, and the step monotonically shrinks and rotates toward the steepest-descent direction as λ climbs. Nothing about the beacons, the measurements, or the current guess changed between rows; λ is a knob the solver turns entirely on its own, based only on whether the previous step actually reduced the cost.
Using the exact JTJ and JTr from the hand-worked Gauss-Newton iteration in Chapter 3, drag λ and watch the step arrow (orange) shrink and rotate from the full Gauss-Newton step toward the pure steepest-descent direction (teal dashed) as damping increases.
calibrateCamera and almost every robotics back-end (Ceres Solver, g2o, GTSAM) use Levenberg-Marquardt as their default solver, not plain Gauss-Newton, precisely because real starting guesses (from feature detectors, wheel odometry, or a rough manual measurement) are rarely as close to the truth as a textbook example. Camera calibration in particular starts from a coarse initial guess of focal length and distortion, where pure Gauss-Newton would frequently diverge; LM's automatic damping keeps early iterations cautious and only accelerates once the fit is already close, which is exactly where Gauss-Newton is at its most reliable.| λ value | Behavior | Step character |
|---|---|---|
| λ = 0 | Pure Gauss-Newton | Long, fast, can overshoot far from a good linear approximation |
| λ small | Mostly Gauss-Newton | Slightly shortened, slightly safer |
| λ large | Mostly gradient descent | Short, cautious, points toward steepest descent |
| λ → ∞ | Pure gradient descent | Vanishingly small step, guaranteed (eventually) to reduce cost |
Our beacon problem only ever had two unknowns, x and y, so its normal-equations matrix was a tiny 2×2 block that we solved by hand in a few lines. Real bundle adjustment problems are nothing like that small. A drone mapping a construction site might estimate 200 camera poses (6 unknowns each: 3 for position, 3 for orientation) and 50,000 tracked 3D landmark points (3 unknowns each). That is 200×6 + 50000×3 = 151200 unknowns. Building a dense JTJ matrix that size and inverting it directly would need roughly 23 billion entries and would never finish on any computer.
The rescue comes from noticing that almost all of those entries are exactly zero. A single pixel observation only ever involves one camera and one landmark, never two landmarks or two cameras at once. So each row of the Jacobian J has nonzero entries in just one camera's 6 columns and one landmark's 3 columns, and zeros everywhere else. When you form JTJ, this structure survives: it comes out as a matrix with a dense block for camera-camera interactions, a dense block for landmark-landmark interactions (but only along the diagonal, since two different landmarks never share an observation), and a sparse off-diagonal block connecting cameras to the landmarks each camera actually saw. This shape is called an arrowhead or bordered-block-diagonal pattern.
Each colored cell is a nonzero block. Notice landmark-landmark blocks (bottom right) only ever touch the diagonal: two landmarks never appear in the same observation. Slide the counts up and watch how quickly the naive dense matrix would explode while the actual nonzero pattern stays sparse.
The Schur complement exploits this shape directly. Since landmark blocks only connect to the cameras that observed them, and never to each other, you can algebraically eliminate every landmark variable first, cheaply, one small block at a time, leaving behind a much smaller "reduced camera system" that only involves the camera unknowns. Solve that small, dense camera-only system first (only a few hundred to a few thousand unknowns, not hundreds of thousands), then plug the camera solution back in to recover every landmark's update with more cheap, independent, per-landmark solves.
Put a number on "dramatically faster." Solving a dense n×n system by the usual method costs on the order of n3 arithmetic operations. Plug in our 151,200-unknown drone example directly and that is roughly 3.5×1015 operations, years of computation on ordinary hardware. Eliminate the 150,000 landmark unknowns first with the Schur complement and you are left with a dense system in only the 200×6 = 1200 camera unknowns, costing on the order of 12003 ≈ 1.7×109 operations for that stage, plus a handful of cheap independent 3×3 solves (one per landmark) to recover every landmark afterward. That is roughly a millionfold reduction in the expensive part of the computation, and it is why bundle adjustment on tens of thousands of landmarks finishes in seconds rather than never finishing at all.
| Approach | Cost of the expensive dense solve | Practical outcome on the drone example |
|---|---|---|
| Dense, no sparsity exploited | O(n3) with n = 151,200 | ≈3.5×1015 ops: infeasible |
| Schur complement, cameras only | O(m3) with m = 1,200 camera unknowns | ≈1.7×109 ops, plus cheap per-landmark solves: seconds |
Recall from Chapter 2 that squared cost punishes a large residual far more than a small one: a residual of 10 costs one hundred times more than a residual of 1. That growth rate is exactly right when every measurement really does follow the same Gaussian noise model. But real sensors occasionally fail in ways that are not Gaussian at all: a beacon reflection off a metal cabinet can add ten extra meters to a range reading in an instant, a camera feature detector can mismatch two unrelated points on two different objects. One such outlier, squared, can dominate the entire cost function and drag the whole solution far from the truth, even though every other measurement is fine.
The fix is to change the shape of the penalty for large residuals while leaving it unchanged for small ones. A robust kernel (sometimes called a robust loss) replaces the plain square with a function that still behaves like a square near zero, where Gaussian noise really does live, but grows much more slowly, even linearly or logarithmically, once the residual gets large. The two most common choices are the Huber kernel and the Cauchy kernel:
Huber is exactly quadratic up to a threshold δ, then switches to growing linearly instead of quadratically: a residual of 10 with δ = 1 costs about 9.5, not 100. Cauchy is even gentler: it grows only logarithmically, so even a wildly wrong measurement contributes a bounded, small nudge instead of dominating the fit. Both kernels agree with plain squared cost for small, well-behaved residuals, which is exactly what you want: do not punish the honest measurements differently, only tame the dishonest ones.
Drag the residual slider and watch where it lands on each curve. Near zero, all three curves are nearly identical. Far from zero, squared cost (red) rockets upward while Huber (warm) bends into a straight line and Cauchy (teal) flattens out almost entirely.
In practice, robust kernels are implemented through Iteratively Reweighted Least Squares (IRLS): at every Gauss-Newton or LM iteration, compute each residual's current size, look up how much that kernel would down-weight a residual of that size, and multiply that residual's row of the Jacobian and its own residual value by the square root of that weight before forming the normal equations. A residual sitting comfortably inside the well-behaved region gets a weight near 1 (full trust, business as usual); a wild outlier gets a weight near 0 (its vote in the fit is silenced without ever needing to be manually deleted from the dataset).
For Huber with threshold δ, the weight formula is exactly weight = 1 when |r| ≤ δ, and weight = δ / |r| once the residual exceeds the threshold. Take a worked case: δ = 1 and a beacon reporting a residual of r = 4 (the multipath-corrupted beacon from Chapter 0's kind of scenario). That residual's weight is 1 / 4 = 0.25. Inside the normal equations, that beacon's row of the Jacobian and its residual both get scaled down as if the measurement were four times less certain than a well-behaved one, so its contribution to the final answer is one quarter of what plain squared cost would have given it. Push the residual out to r = 40 and the weight drops to 1/40 = 0.025, a fortieth of a normal measurement's vote: the worse the outlier, the more automatically it gets silenced, with no manual threshold to tune beyond choosing δ itself.
So far this lesson has treated the final answer, a single position (x, y), as the whole story. But an estimate without a sense of how trustworthy it is can be dangerous: a self-driving car should behave very differently when it is confident it is centered in its lane versus when its own estimate says it might be anywhere within a meter. Nonlinear least squares hands you that confidence for free, buried inside the same JTJ matrix you already built for every Gauss-Newton step.
At the optimum, the matrix JTJ is (up to a noise-scale factor) an approximation of the Hessian, the matrix of second derivatives of the cost surface, which measures how sharply the bowl curves upward around its minimum. A sharply curving bowl (large JTJ) means even a small nudge away from the optimum costs a lot, so the optimum is well pinned down: low uncertainty. A shallow, nearly flat bowl (small JTJ) means you can wander quite far from the optimum before the cost changes much, so the optimum is poorly pinned down: high uncertainty. Inverting that curvature turns "how steep" into "how uncertain," giving the estimated covariance:
The σ2 out front is the per-measurement noise variance, the same quantity from Chapter 2's Gaussian noise model, and it is easy to drop by accident. If your beacon ranges have a noise standard deviation of, say, 0.3 meters, then σ2 = 0.09, and the true covariance is only nine percent as large as the raw (JTJ)-1 would suggest on its own. Skipping this factor is a common and dangerous shortcut: it reports uncertainty in the wrong units entirely, confidently wrong rather than honestly uncertain. For the worked numbers below we set σ2 = 1 to keep the arithmetic clean, which is the same as saying every residual is already measured in units of one noise standard deviation; scale the final ellipse by your sensor's actual noise variance before trusting it in real meters.
This is the exact same 2×2 inverse machinery from Chapters 1, 3, and 4. Reusing the matrix from our worked Gauss-Newton iteration, JTJ = [[1.5,-0.5],[-0.5,1.5]], its inverse (computed the same way as in Chapter 3, determinant 2.0) is (1/2)[[1.5,0.5],[0.5,1.5]] = [[0.75, 0.25],[0.25, 0.75]]. That matrix says: the variance along x is 0.75, the variance along y is 0.75, and there is a positive correlation of 0.25 between errors in x and errors in y, all in whatever squared-range units the residuals were measured in. Plotted as an ellipse, this describes a tilted oval of likely positions around the estimate, not a perfect circle, because the three beacons in this particular layout do not surround the guess symmetrically enough to pin down every direction equally.
Drag beacon three (purple) around the room. Watch the uncertainty ellipse (computed live from (JTJ)-1 at the true position) shrink toward a tight circle when the three beacons surround the position from well-spread angles, and stretch into a long, dangerous sliver when beacon three drifts into nearly the same line as beacons one and two.
This effect has a name in the GPS world: dilution of precision (DOP). When satellites are spread across the whole sky, ranges pin down your position tightly in every direction, low DOP, high confidence. When satellites cluster together in one patch of sky, most possible position errors barely change any of the ranges, high DOP, low confidence, even though the measurements themselves might be just as accurate as before. The uncertainty ellipse you just dragged around is DOP made visible.
Everything from the last seven chapters lives in one lab. Drag the three beacons anywhere on the floor. Drag the true robot position. Add measurement noise, inject an outlier on demand, then step Gauss-Newton or Levenberg-Marquardt one iteration at a time and watch the guess (orange) march down the cost surface toward the truth (teal), leaving a trail of every past guess behind it. The background heatmap is the actual cost function J(x, y) from Chapter 2, evaluated at every point on the floor: darker means lower cost, and the point you are hunting for is the darkest spot on the whole floor.
Try this sequence: leave the outlier off and step through five or six iterations with λ = 0. The guess should slide smoothly and quickly into the darkest well of the heatmap, matching what Chapter 3's hand-worked example predicted. Now drag a beacon to make the three beacons nearly collinear and reset: notice the cost bowl flattens into a long valley (echoing Chapter 7's stretched ellipse), and pure Gauss-Newton steps start overshooting back and forth across the valley instead of walking straight down it. Raise λ and re-run: the steps shrink and the path calms down, trading speed for stability exactly as Chapter 4 described. Finally, turn the outlier on with Huber off: watch one bad measurement visibly warp the whole heatmap and pull the true minimum away from the real position. Flip Huber on and re-run: the heatmap and the found minimum snap back close to the true position, because the robust kernel is discounting the corrupted measurement's vote, just as Chapter 6 explained.
You now have the full toolbox: an exact one-shot solver for straight-line problems, an iterative solver for curved ones, a damping knob that trades speed for safety, a way to survive outliers, and a way to report how much to trust the answer. Here is the map for deciding which piece to reach for.
| Method | Is the model linear in the unknowns? | Core question it answers | Reach for it when |
|---|---|---|---|
| Linear least squares | Yes | What single line or plane best fits these over-determined measurements | The model really is linear (sensor calibration, simple curve fitting) |
| Gauss-Newton | No, but a good starting guess exists | What point minimizes squared nonlinear residuals | You already have a decent initial estimate (tracking, refinement, bundle adjustment near convergence) |
| Levenberg-Marquardt | No, and the starting guess may be rough | Same as Gauss-Newton, plus how far to trust the linear approximation this step | The default choice for almost any nonlinear least squares problem in practice |
| Sparse / Schur complement | No, and the problem has thousands of unknowns | How to solve the same normal equations without wasting time on structural zeros | Bundle adjustment, large factor-graph SLAM, any problem where variables interact only in small local groups |
| Robust kernels (Huber, Cauchy) | No, and some measurements are simply wrong | How to fit the majority of good data without letting outliers hijack the answer | Real sensors: LIDAR scan matching, feature-based vision, any pipeline with occasional bad correspondences |
| Covariance from (JTJ)-1 | Reused from whichever solver already ran | How much to trust the answer, and in which directions | Reporting accuracy to a downstream consumer: a planner, a human, a safety check |
Every idea from Chapters 3 through 7, Gauss-Newton's linearize-solve-repeat, Levenberg-Marquardt's damping, robust reweighting, and the covariance readout, fits together into a single loop that real solvers (Ceres, g2o, GTSAM) run underneath the hood:
python (the complete robust damped loop) def solve(beacons, z, x0, lam0=1.0, huber_delta=1.0, iters=20): x, lam = x0, lam0 cost = total_cost(beacons, z, x, huber_delta) for _ in range(iters): # build the (optionally reweighted) normal equations at x JtJ, Jtr = build_normal_equations(beacons, z, x, huber_delta) # damp, then solve for the candidate step (Ch3 + Ch4) dx = solve_2x2(JtJ[0][0]+lam, JtJ[0][1], JtJ[1][0], JtJ[1][1]+lam, -Jtr[0], -Jtr[1]) x_try = (x[0]+dx[0], x[1]+dx[1]) new_cost = total_cost(beacons, z, x_try, huber_delta) if new_cost < cost: # step helped: accept it, trust more x, cost = x_try, new_cost lam = max(lam / 3, 1e-7) else: # step hurt: reject it, trust less lam = lam * 3 covariance = invert_2x2(*JtJ) # Ch7: uncertainty at the final x return x, covariance
Read it as a story: guess, build the local linear model, ask the damping knob how bold to be, take the step only if it actually helped, otherwise get more cautious and try again, repeat until the step is too small to matter, then read off both the answer and how much to trust it. Every simulation you dragged a slider on in this lesson was a piece of that loop, isolated so you could see it work on its own.
| Symptom | Likely cause | Fix |
|---|---|---|
| Solver diverges or oscillates | Starting guess too far away, or λ too small | Raise λ, or find a better initial guess (e.g. a closed-form linear approximation first) |
| Solution looks confident but wrong | An outlier dominated the squared cost, or the model itself is mis-specified | Add a robust kernel; sanity-check the model against a known ground truth |
| Reported covariance is enormous | Poor measurement geometry (near-collinear beacons, satellites clustered in the sky) | Add measurements from a different geometry, or accept and report the honest uncertainty |
| Solver is too slow on a large problem | A dense solve was used on a sparse structure | Exploit sparsity with a Schur-complement or sparse Cholesky solver |
You started this lesson unable to explain why three noisy circles refuse to meet at one point. You end it able to derive the step that finds the best point anyway, by hand, and to say exactly when that step should be trusted and when it should be damped, robustified, or doubted.