The Sensor Suite
& Its Failure Modes

Chapter 1: Cameras — Projection, Distortion, and the Shutter That Was Never One Instant

A camera is the cheapest, densest, most informative sensor you can bolt to a robot, and it is the one engineers most often think they understand. The forward model is genuinely simple. The three things that bite are all on the inverse side, and all three show up in the field.

The model is easy, the inverse is not

Here is the forward pipeline in the form you should be able to write without hesitating, because where in the sequence you put the distortion is the part people get wrong.

Read the arrows carefully. Distortion acts on normalised coordinates, between the perspective divide and the multiplication by K. Getting that order wrong is the single most common silent error in hand-rolled camera code, and it is the reason someone's calibration "works" at the image centre and falls apart at the edges. If you apply D after K, then k₁ has units of inverse pixels squared and every value in the literature is wrong by f².

The Brown-Conrady model — the one behind cv2.calibrateCamera, ROS camera_info, Kalibr and almost everything else — splits D into a radial part and a tangential part:

Every symbol: x, y are the ideal pinhole normalised coordinates (dimensionless, equal to the tangent of the angle from the optical axis). r is radial distance from the principal point in those units. k₁, k₂, k₃ are radial coefficients — the even-powered series comes from the fact that a rotationally symmetric lens can only produce a function of r², and it must be odd in the displacement, so the displacement is r times an even series. p₁, p₂ are tangential (decentring) coefficients, non-zero only because the lens elements are not perfectly coaxial with the sensor.

Worked example 1 — forward, by hand

Take a realistic 1280×720 wide-angle camera: f_x = f_y = 600 px, c = (640, 360), k₁ = −0.28, k₂ = 0.07, and ignore tangential terms for the hand pass. Project the normalised point (x, y) = (0.35, 0.20). That direction is about 19.3° off axis horizontally — roughly two thirds of the way to the edge of the frame.

1. r² = 0.35² + 0.20² = 0.1225 + 0.0400 = 0.1625
2. r⁴ = 0.1625² = 0.02640625
3. k₁r² = −0.28 × 0.1625 = −0.045500
4. k₂r⁴ = 0.07 × 0.02640625 = 0.00184844
5. radial factor = 1 − 0.045500 + 0.00184844 = 0.9563484
6. x_d = 0.35 × 0.9563484 = 0.3347220,   y_d = 0.20 × 0.9563484 = 0.1912697
7. u = 600 × 0.3347220 + 640 = 200.833 + 640 = 840.833,   v = 600 × 0.1912697 + 360 = 114.762 + 360 = 474.762

The ideal pinhole pixel would have been (600 × 0.35 + 640, 600 × 0.20 + 360) = (850.0, 480.0). So the lens pulled this point 9.17 px left and 5.24 px up, a radial displacement of √(9.17² + 5.24²) = 10.56 px. Ten pixels is not a rounding error — it is roughly thirty times the sub-pixel precision of a good corner detector, and it is why nobody triangulates on raw pixels.

Worked example 2 — the inverse has no closed form

Now the question that actually gets asked: you have the pixel (840.833, 474.762) and you want the ray it came from. Undistort it.

You cannot solve x(1 + k₁r² + k₂r⁴) = x_d for x in closed form, because in terms of r it is a degree-5 polynomial (degree 7 with k₃) and there is no general radical solution above degree 4. Almost every library therefore runs a fixed-point iteration: guess that the undistorted radius is the distorted radius, evaluate the distortion factor there, divide it out, repeat.

Run it on our point. Start with (x_d, y_d) = (0.3347220, 0.1912697), so r²₍₀₎ = 0.1486229.

Each row shrinks the error by roughly a factor of 11.5. That ratio is not magic: near the fixed point the iteration contracts at a rate equal to the derivative of the map, which here is about |2k₁r²| / (1 + k₁r²) ≈ 0.0910/0.956 ≈ 0.095. One digit of pixel accuracy per iteration, near enough.

The shutter, and why "the image" was never one instant

Here is the assumption buried in every equation above: that all the pixels in the frame saw the world at the same moment. For a global shutter sensor, which transfers every pixel's charge to a shielded storage node simultaneously, that is true. For a rolling shutter sensor, which resets and reads the array one row at a time, it is false, and the amount by which it is false is a number you can compute.

Let the readout of the whole frame take t_r seconds over N_rows rows. Row v is exposed centred at

so the camera is not a projection from one pose but from a continuum of poses. The correct model is

where T(t) is the camera trajectory. That is an implicit equation — the row you land on determines the pose you should have used to compute the row — and solving it is why rolling-shutter-aware bundle adjustment is materially harder than the global-shutter kind.

Worked example 3 — how much skew, exactly

An AMR turns a corner at 60°/s = 1.047198 rad/s. The camera is 1920×1080 at 30 fps with a rolling shutter whose readout spans the full frame period, t_r = 33.33 ms. Focal length f = 600 px. A vertical pole sits in front of the robot. How tilted does it look?

1. Per-row time: Δt = 33.33 ms / 1080 = 30.864 µs.
2. Angular change per row: ωΔt = 1.047198 × 30.864e−6 = 3.2321 × 10^-5 rad.
3. Horizontal image shift per row: f · ωΔt = 600 × 3.2321e−5 = 0.019393 px per row. (For small angles about the optical axis's perpendicular, image displacement is f × angle — no depth dependence at all.)
4. Shift from top row to bottom row: 0.019393 × 1080 = 20.94 px.
5. Apparent tilt of the pole: arctan(20.94 / 1080) = 1.111°.

One degree does not sound like much. Now ask what it does downstream. If that pole is a vertical line feature feeding a heading estimate, you have just injected 1.1° of heading error that appears only while turning at 60°/s. Slow down to 10°/s and it drops to 0.19°. Stop and it vanishes. An error that scales with angular rate and disappears at rest is indistinguishable from a gyro scale-factor error unless you know to look for it.

The rotation term has a property that makes it especially nasty: it does not depend on depth. Every pixel in the frame, near or far, shears by the same amount. Translation-induced rolling-shutter distortion does depend on depth — the image velocity of a point at depth Z under lateral speed v is f·v/Z. At v = 1.5 m/s and Z = 3 m with f = 600, that is 600 × 1.5 / 3 = 300 px/s, so over the same 33.3 ms readout the near object shears by 10 px while a point at 30 m shears by 1 px. Rotation shears everything uniformly; translation shears the foreground.

What to look for. With lateral speed at zero, every object in the frame — the near pole and the far pole — leans by exactly the same angle. Now set yaw to zero and lateral speed to 3 m/s: the near pole shears hard and the far pole barely moves. That difference is the discriminator. If a colleague shows you a skewed image and asks whether it is rotation or translation, the answer is in whether the skew is depth-dependent.

Where a camera sits in a real stack

The exercise that proves you own this: draw the camera path from photons to features, and put numbers on it. Here is the path, with the numbers from our AMR.

The one decision in that diagram worth defending. Notice that nothing undistorts the image. Undistorting a full 1080p frame means a bilinear resample of 2.07 M pixels driven by a remap table; stored as two float32 channels that table is 16.6 MB, and even in 16-bit fixed point it is 8.3 MB — far outside any L2 cache, so the operation is memory-bound and costs about 4–6 ms per frame on a Jetson-class CPU, plus 250–500 MB/s of extra memory traffic at 30 Hz. Undistorting the 400 points you actually use costs microseconds. The rule: undistort the sparse thing, never the dense thing, unless something downstream genuinely needs a rectified image (dense stereo does; a feature tracker does not).

The latency budget, end to end. Photons land over the 10 ms exposure, so the honest capture time is exposure start + 5 ms. The last row leaves the sensor 33.3 ms after the first. Debayer and any ISP work costs about 3 ms. The DMA hand-off and the pool copy cost 0.4 ms. Feature extraction costs 5 ms. Total from mid-exposure to a usable feature set: 5 + 33.3 + 3 + 0.4 + 5 ≈ 46 ms. At 1.5 m/s the robot has travelled 6.9 cm in that time, and at 3 m/s it has travelled 13.8 cm. That number is the reason the estimator must consume a timestamped measurement and never "the latest image" — see Time, Clocks & Sensor Alignment for what happens when it does not.

From scratch, then the one-liner

The from-scratch version, which is what you write when OpenCV is not on the table:

python
import numpy as np

def distort(xn, yn, k1, k2, k3, p1, p2):
    # xn, yn are NORMALISED (already divided by Z). Never pass pixels here.
    r2 = xn * xn + yn * yn
    radial = 1.0 + k1 * r2 + k2 * r2 ** 2 + k3 * r2 ** 3
    xd = xn * radial + 2 * p1 * xn * yn + p2 * (r2 + 2 * xn * xn)
    yd = yn * radial + p1 * (r2 + 2 * yn * yn) + 2 * p2 * xn * yn
    return xd, yd

def undistort_points(u, v, K, dist, iters=15):
    # pixels -> normalised distorted -> normalised ideal, by fixed point
    k1, k2, p1, p2, k3 = dist
    xd = (u - K[0, 2]) / K[0, 0]
    yd = (v - K[1, 2]) / K[1, 1]
    x, y = xd.copy(), yd.copy()          # guess: the distorted point itself
    for _ in range(iters):
        r2 = x * x + y * y
        radial = 1.0 + k1 * r2 + k2 * r2 ** 2 + k3 * r2 ** 3
        dx = 2 * p1 * x * y + p2 * (r2 + 2 * x * x)
        dy = p1 * (r2 + 2 * y * y) + 2 * p2 * x * y
        x = (xd - dx) / radial           # subtract tangential, divide out radial
        y = (yd - dy) / radial
    return x, y

Three things to keep straight while you type it, because they carry most of the understanding. One: "distortion lives in normalised coordinates, so I strip K first." Two: "there is no closed form, so this is a fixed point; I initialise at the distorted point because the map is a mild contraction." Three: "fifteen iterations is overkill — five gets me to a twentieth of a pixel — but it is branch-free and vectorises, which matters more than the flop count."

And the production form, which you should also know exists and what it costs:

python
import cv2
# sparse: 400 points, microseconds. P=None means "return normalised", not pixels.
xy = cv2.undistortPoints(pts.reshape(-1, 1, 2), K, dist).reshape(-1, 2)

# dense: build the map ONCE at startup, then remap each frame (4-6 ms at 1080p)
map1, map2 = cv2.initUndistortRectifyMap(K, dist, None, Knew, (w, h), cv2.CV_16SC2)
rect = cv2.remap(img, map1, map2, cv2.INTER_LINEAR)

Note CV_16SC2 in that call. That is the fixed-point map format, and choosing it over CV_32FC2 halves the map from 16.6 MB to 8.3 MB. On a memory-bound operation that is close to a 2× speed-up for free, and knowing it is the kind of detail that reads as "has actually profiled this".

Two failures, and the metric that separates them

Failure 1: the calibration that reports 0.19 px and still bows at the corners

Symptom. calibrateCamera returns an RMS reprojection error of 0.19 px. Everyone is pleased. But a straight door frame near the image edge is visibly curved in the undistorted image, and a stereo rig built on this calibration produces depth that is 4% wrong in the outer third of the frame and correct in the middle.

Root cause. The checkerboard never went near the corners. The optimiser fitted k₁, k₂ and k₃ to data spanning r ∈ [0, 0.42] and is now being evaluated at r = 0.78. A degree-6 polynomial extrapolated 85% beyond its data does whatever it likes, and the reported RMS — which is computed only on the observed corners — is blind to all of it.

The metric that reveals it. Not the RMS. Two things:

• Radial coverage histogram. Bin every detected corner by its radial distance r and plot the count. If the outermost populated bin is at r = 0.42 while the image corner is at r = 0.78, you have a coverage hole and the number to quote is the fraction of image area with zero calibration observations — here roughly 45%.
• Reprojection error binned by radius. A healthy calibration has a flat residual-versus-r curve. A rising tail in the outermost populated bin means the model is already straining where you do have data, which predicts what it does where you do not.

The decoy to rule out. The nearest wrong answer is "the distortion model is the wrong order — add k₃, or drop to k₁ only". The discriminator: refit with and without k₃ and compare two numbers. If RMS barely moves (0.19 → 0.18 px) but the predicted corner displacement changes by several pixels, the higher-order term is unconstrained by the data, and the fix is more board coverage, not more parameters. If instead RMS drops materially, the model really was too simple. One refit, two numbers, unambiguous answer.

Failure 2: the camera-IMU extrinsic that "drifts with speed"

Symptom. Your VIO's online extrinsic estimate for the camera-to-IMU rotation is stable at rest and during straight driving, but shifts by up to a degree during turns, and always in the direction of the turn. Someone proposes that the bracket is flexing.

Root cause. Rolling shutter. The features are spread over the image, so on average they were captured at t_r/2 after the frame timestamp. The estimator attributes that half-readout time offset to whatever free parameter can absorb it, and the camera-IMU rotation is the most flexible one available.

The metric that reveals it. Bucket the log by mean angular rate and plot estimated extrinsic yaw offset against |ω|. A mechanical flex would correlate with linear acceleration or with temperature, and would show hysteresis. A shutter artefact gives a straight line through the origin whose slope is t_r/2. With t_r = 33.3 ms the predicted slope is 16.7 ms, so at ω = 60°/s you expect an apparent offset of 60 × 0.0167 = 1.0° — and that matches the observation. Predicting the slope from the datasheet before looking at the plot is what turns a guess into a diagnosis.

What is changing

Global shutter stopped being expensive. The classic argument for tolerating rolling shutter was cost and pixel size. Sony's Pregius S generation (2019 onward) put back-illuminated global-shutter pixels at 2.5 µm, which removed most of the resolution and sensitivity penalty. If you are specifying a new robot camera in 2026 and you have rotation in your motion profile, "we chose rolling shutter to save money" is a position you now have to defend rather than assume.

The twelve-parameter model is not sacred. Schöps, Larsson, Pollefeys and Sattler, "Why Having 10,000 Parameters in Your Camera Model Is Better Than Twelve" (CVPR 2020), showed that a dense per-pixel or per-region camera model, fitted with a proper calibration procedure, reduces reprojection error well below what Brown-Conrady can reach — because real lenses are not rotationally symmetric polynomials, and the residual structure a global model leaves behind is systematic, not noise. The practical takeaway: if your residuals show spatial structure rather than white scatter, the model is the limit, not the data.

Event cameras remove the shutter entirely. Each pixel independently reports log-intensity changes with microsecond timestamps, so there is no frame, no exposure and no readout order. Gallego et al., "Event-based Vision: A Survey" (IEEE TPAMI, 2022) is the reference; the properties that matter for robotics are ~1 µs temporal resolution, over 120 dB of dynamic range, and no motion blur. The cost is that every algorithm you know assumes frames, so you are rebuilding the front end.

Rolling shutter as a first-class model. Schubert, Demmel, Usenko, Stückler and Cremers, "Direct Sparse Odometry with Rolling Shutter" (ECCV 2018), showed that treating the shutter properly — a continuous-time trajectory rather than one pose per frame — recovers most of the accuracy lost to a rolling sensor. This is the same continuous-time idea that Chapter 3 uses to de-skew a LiDAR sweep, and noticing that the two problems are identical is a genuinely strong thing to say out loud.

Chapter 2: Stereo & Depth — Where the Quadratic Bites, and Where Depth Simply Dies

One camera throws depth away. Two cameras get it back — but not uniformly, and not everywhere. A stereo rig is precise up close and progressively useless with range, and the exponent in "progressively" is the number that decides designs.

Disparity, and why the error is quadratic

Two cameras with parallel optical axes, identical focal length f (in pixels), separated by a baseline B (in metres). A world point at depth Z projects into the left image at u_L and the right at u_R. The disparity is d = u_L − u_R, measured in pixels.

Similar triangles give the relation in one line. The point sits at horizontal offset X from the left camera and X − B from the right, so u_L = fX/Z + c and u_R = f(X − B)/Z + c, and subtracting kills X and c entirely:

Notice what disappeared: the point's lateral position, the principal point, and any common shift of both cameras. Disparity depends on depth and nothing else. That is the whole reason stereo works, and it is also the reason a stereo rig is exquisitely sensitive to anything that breaks the "identical, parallel, rectified" assumption — a relative rotation between the two cameras does not cancel out.

Now differentiate. This is the step most people skip, and it is the whole result. Write Z as a power of d first, because then it is one line of calculus with no quotient rule:

That is still expressed in disparity, and the useful form is in range. So substitute d = fB/Z and write out every intermediate expression — this is the step that usually gets hand-waved:

Squaring fB/Z put (fB)² in the denominator and Z² in the numerator; the single fB out front then cancels one of the two factors of fB below. That one cancellation is the entire trick. Take the magnitude and multiply by the matching noise:

Depth uncertainty is quadratic in range. Doubling the distance quadruples the error. Nothing about better matching changes the exponent; it only changes σ_d.

Keep one caveat in your pocket, because it returns in Worked example 4: this is a linearisation, and it is only trustworthy while the disparity error is small next to the disparity itself. At Z = 10 m on the rig below, d = 8.4 px and the derivative claims 10²/84 = 1.190 m of depth per pixel of disparity. The exact difference across a whole 0.244 px of error is 84/8.4 − 84/8.644 = 0.2827 m, which is 1.157 m per pixel — 2.8% lower, because Z(d) is convex and the tangent line always sits above the true curve. Under about half a pixel the two agree to a few percent; at several pixels they diverge badly and you should quote the exact form and say so.

Worked example 1 — the table you should be able to produce cold

Take a realistic robot stereo head: f = 700 px, B = 0.12 m, and a good sub-pixel matcher achieving σ_d = 0.2 px. Then fB = 84.0 px·m, and σ_Z = Z² × 0.2 / 84.0 = Z² / 420.

Read the disparity column. At 20 m the entire signal is 4.2 pixels wide. A half-pixel matching bias — entirely achievable with a rectification error, a slight defocus, or a slanted surface — is a 12% depth error. Stereo does not fail at long range because the algorithm gets worse; it fails because the measurement itself shrinks to nothing.

The inversion is the useful engineering move. If the requirement is "depth to 10 cm at 10 m", then 0.10 = 100 × σ_d/(fB), so you need fB ≥ 1000σ_d = 200 px·m. At f = 700 that is B ≥ 0.286 m. So: "To hit ten centimetres at ten metres with this lens I need about a thirty-centimetre baseline." That sentence, produced in fifteen seconds, is worth more than any amount of qualitative discussion.

Worked example 2 — what a longer baseline costs you

A longer baseline is not free. Three things get worse and you should name all three.

Minimum range. The matcher searches a finite disparity range 0 … d_max. Anything closer than Z_min = fB/d_max is simply outside the search and returns nothing. With d_max = 128:

The 0.30 m rig meets the 10 cm requirement and cannot see anything closer than 1.64 m. On a warehouse AMR that has to detect a pallet jack tine at 0.8 m, that trade is fatal — which is exactly why real robots carry two depth modalities rather than one perfect one.

Occlusion. Anything visible to one camera and hidden from the other has no match, and the geometry tells you exactly how wide that dead band is. Both surfaces shift left in the right image by their own disparity; the foreground shifts further because its disparity is larger, so it slides over a strip of background. The width of that strip in the reference image is therefore the disparity difference across the edge, d_fg − d_bg — not the foreground disparity outright. For a 1 m object against a distant wall with B = 0.12 and f = 700 that is 84.0 − 0 = 84 px, which is where the table's last column comes from; for the pallet-and-aisle case used later in this chapter (2.0 m foreground, 8 m background) it is 42.0 − 10.5 = 31.5 px. Only when the background is effectively at infinity does the band equal the foreground disparity. "Double the baseline, double the band" still holds, because both disparities scale with B and so does their difference.

The last column is the one that matters operationally: multiply the band in pixels by Z_bg/f and you get the metres of background surface that no stereo rig with this baseline can ever see. A 0.36 m strip of aisle floor hidden behind every pallet leg is a permanent hole in the costmap, not a tuning problem, and no amount of algorithm work removes it — only a different viewpoint does.

One more fact worth volunteering, because it is a free debugging tell: with the left camera as reference the occlusion bands appear on the left side of foreground objects, since that is the background the rightward-displaced camera loses. If your invalid-pixel mask hugs the right edges instead, you have swapped the two images or the sign of your disparity convention, and every depth in the frame is wrong in a way that still looks plausible.

Overlap and rectification error. A wider baseline means a smaller shared field of view and a longer, more flexible mechanical structure between the two sensors. A 0.02° relative yaw between the cameras shifts disparity by f × tan(0.02°) = 700 × 3.49e−4 = 0.24 px uniformly — which at 10 m is a 28.3 cm depth bias affecting every pixel identically. That is not noise, it is a scale error in your entire depth map, and it does not average away.

Where depth dies, in four distinct ways

"Stereo needs texture" is the answer everybody gives. It is true and it is one quarter of the answer. There are four failure geometries, they produce different artefacts, and they need different fixes.

The last row is the confident-wrong class from Chapter 0, and it is why the two obvious quality metrics are not enough. A specular highlight on a metal shelf appears at a different physical location in each image, so the matcher finds a strong, unique, left-right-consistent match at a disparity that corresponds to no surface at all. Every internal check passes.

Worked example 3 — why active depth dies outdoors

An active stereo or structured-light camera projects a near-infrared pattern to manufacture texture. The design question is whether the projected pattern is brighter than the ambient background within the receiver's optical passband. Work the photon budget.

1. Solar spectral irradiance at ground level near 850 nm is about 0.95 W/m² per nm. Through a 30 nm narrowband filter, the scene receives 0.95 × 30 = 28.5 W/m² of ambient light the camera cannot reject.
2. The projector emits about 1 W of optical power over a 60° × 45° field. At 2 m that field covers 2 × 2 × tan(30°) = 2.31 m wide by 2 × 2 × tan(22.5°) = 1.66 m tall, an area of 3.83 m².
3. Projected irradiance on the scene: 1 / 3.83 = 0.261 W/m².
4. Pattern contrast in full sun: 0.261 / 28.5 = 0.0092, that is 0.92% modulation on top of a bright background — buried under the shot noise of the background itself.
5. The same projector indoors, where the in-band ambient is around 0.005 W/m²: contrast = 0.261 / 0.005 ≈ 52× the background. Overwhelming.

That is a 5000-fold swing in the thing that matters, and it is why the identical camera is superb in a warehouse and returns nothing in a yard at noon. The physics also tells you the only three levers: more projector power (limited by eye safety and heat), a narrower filter (limited by the emitter's linewidth and its thermal drift), or a shorter exposure with a pulsed projector so the ambient integrates for less time. Naming those three, in that order, is a complete answer.

What to look for. Set the requirement in your head — say 10 cm at 10 m — and drag the baseline until the teal curve crosses the 10 m gridline below 0.10 m. Now look at where the grey blind zone has moved to. That is the entire stereo design conversation, and the bench makes it impossible to pretend the two ends are independent.

Worked example 4 — a hundredth of a degree, and where it lands

Anyone who has shipped a stereo rig learns to worry about rectification. Here is the arithmetic that makes the worry concrete. Suppose the two cameras have a relative yaw error of δ = 0.02° that your calibration did not capture — a bracket that warmed up, a bolt that settled. That rotation shifts every match horizontally by f·tan(δ) = 700 × 3.4907e−4 = 0.244 px, uniformly across the image.

Which model the table uses, and why the two disagree. Every entry above is the exact form fB/d − fB/(d + δd): at 10 m that is 84/8.4 − 84/8.6443 = 10.000 − 9.717 = 28.3 cm. The linearised form derived earlier in this chapter, Z²·δd/(fB) = 100 × 0.2443/84, gives 29.1 cm. Both are defensible and the 3% gap between them is itself the teaching point: it is the signature of a perturbation large enough that the tangent approximation is starting to break down. Quote the linearised number when you are sizing a design (it is conservative — it overestimates, because Z(d) is convex), and the exact number when you are reconciling against a measurement. Saying which one you used, unprompted, is a small thing that reads as rigour.

Two features of that table matter. First, the error is signed and systematic — every pixel in the frame reports depth in the same wrong direction, so it looks like a clean, plausible depth map that is uniformly wrong. Second, it is invisible up close and enormous far away, which means a bench test at 1.5 m passes and the field failure appears at 10 m. Anyone who has debugged this recognises the shape immediately: a robot that grabs boxes perfectly and consistently misjudges the aisle end.

The design consequence is mechanical, not algorithmic: the two imagers must share a rigid, thermally matched mount, and the rig needs an online check. The cheapest one is the vertical disparity residual — after rectification, matched points should have identical row coordinates, so the mean vertical offset across the frame is a free, continuously available measure of rectification health that needs no target.

Choosing among four depth modalities

"Depth camera" is four different physical devices, and picking one is a standard design prompt. The honest comparison:

The row that decides most arguments is the error law. Stereo is better than LiDAR below the crossover — with our f = 700, B = 0.12 rig and a 3 cm LiDAR, stereo wins out to √(0.03 × 84 / 0.2) = 3.55 m — and progressively worse beyond it. That single crossover number, computed live, is a much stronger answer than any list of pros and cons.

What a stereo front end actually costs

"Would you run stereo on the CPU?" is really a question about the size of the cost volume. Compute it.

28 GB/s of memory traffic is roughly the entire DRAM bandwidth of a Jetson Orin NX. That single number answers the question: dense stereo is a memory-bandwidth problem, not a compute problem, and it is why it lives on an FPGA, an ASIC, or a GPU with the cost volume tiled to stay in shared memory. If you must run it on a CPU, the levers in order of effectiveness are: reduce D (which raises Z_min), downsample (which raises σ_d in metres per pixel but lowers W×H quadratically), or drop from 8 aggregation paths to 4 (which costs you accuracy on diagonal structures).

The output-size trap. Emitting a full 1280×720 point cloud at 30 Hz is 0.92 M points × 12 bytes = 11.1 MB per frame, 331 MB/s. Compare that with what you started with: two mono8 images, 2 × 0.92 MB = 1.84 MB per stereo frame, 55.3 MB/s. The cloud is exactly 331/55.3 = 6.0× the raw stereo input. Every experienced perception engineer has watched a team saturate a link by publishing a dense cloud that nothing downstream needed at full density.

The design answer is to publish the disparity image and let each consumer reproject only the region it cares about. Price it honestly, because this is where the arithmetic usually goes wrong: 1280 × 720 = 921,600 pixels, and at uint16 that is 2 bytes each, so 1.84 MB per frame and 55.3 MB/s — the same 1.84 MB you were already carrying for the stereo pair, one channel at two bytes instead of two channels at one. So the disparity image is not a 6× saving over the cloud in the sense of being cheap; it is a 6× saving in the sense that publishing the cloud costs six times what publishing the disparity image costs, and the disparity image is free — it is bandwidth your link already carries.

If you genuinely need it cheaper, the only lever is bits per pixel, and every bit you drop is bought from either disparity range or sub-pixel resolution. The bit budget is fixed by d_max: covering 0…128 px of integer disparity needs 8 bits, and OpenCV's 4 fractional bits need 4 more, so 12 bits — which is why the payload is uint16 and not uint8. Squeeze it into one byte and you must give something up:

Those last two columns come from the quantiser: a uniform step q contributes σ_q = q/√12 pixels, which adds in quadrature with the matcher. At q = 0.5 px that is 0.144 px, so σ_d = √(0.20² + 0.144²) = 0.247 px and σ_Z(10 m) = 100 × 0.247/84 = 0.293 m. At q = 1 px it is 0.289 px, σ_d = 0.351 px, and σ_Z(10 m) = 0.418 m. Halving the bandwidth costs you between a quarter and three quarters of your far-field accuracy — which is a real trade, and a defensible one on a robot that only uses stereo below 5 m, but it is a trade you should make with those two numbers in hand rather than by picking a type that felt small. The genuinely free win is that a disparity image is spatially smooth in a way the raw texture images are not, so lossless compression pays off far better on it; measure the ratio on your own scenes before you budget for it.

Block matching from scratch, then the library

The exercise that makes it stick: implement disparity for one row, with sub-pixel refinement.

python
import numpy as np

def disparity_row(left, right, y, half=3, dmax=64):
    # SAD over a (2*half+1) square window, searching RIGHTWARD in the right image
    H, W = left.shape
    out = np.full(W, np.nan)
    for x in range(half + dmax, W - half):
        ref = left[y-half:y+half+1, x-half:x+half+1].astype(np.float64)
        cost = np.empty(dmax + 1)
        for d in range(dmax + 1):
            tgt = right[y-half:y+half+1, x-d-half:x-d+half+1].astype(np.float64)
            cost[d] = np.abs(ref - tgt).sum()
        d0 = int(cost.argmin())
        if d0 == 0 or d0 == dmax:      # minimum on the edge: no parabola, no trust
            continue
        # --- rejection BEFORE refinement, not after ---
        second = np.min(cost[np.abs(np.arange(dmax+1) - d0) > 2])
        if second < 1.15 * cost[d0]:      # uniqueness ratio: repetitive texture
            continue
        curv = cost[d0-1] - 2*cost[d0] + cost[d0+1]
        if curv <= 1e-6:                # flat valley: textureless
            continue
        # --- sub-pixel: fit a parabola through the three costs ---
        out[x] = d0 - 0.5 * (cost[d0+1] - cost[d0-1]) / curv
    return out

Do the sub-pixel step by hand once so you can say it without the code in front of you. Suppose the SAD costs at d = 23, 24, 25 are 180, 120, 150. The curvature is 180 − 2(120) + 150 = 90. The offset is −0.5 × (150 − 180) / 90 = −0.5 × (−30)/90 = +0.1667. So d* = 24.1667. With f = 700, B = 0.12 that moves the depth from 84/24 = 3.500 m to 84/24.1667 = 3.4759 m — a 24 mm correction from a single sixth of a pixel. Sub-pixel is not polish; at 3.5 m one pixel is 14 cm.

Two things to say while writing it. First: "I reject before I refine — there is no point fitting a parabola to a valley that is either flat or one of several." Second: "the two rejection tests measure different things: the ratio test catches repetitive texture, the curvature test catches no texture, and a real pipeline needs both plus a left-right consistency pass."

The production form, and the parameters that matter:

python
import cv2
sgbm = cv2.StereoSGBM_create(
    minDisparity=0, numDisparities=128,   # sets Z_min = f*B/128
    blockSize=5,
    P1=8 * 5 * 5, P2=32 * 5 * 5,        # smoothness: P1 for +-1, P2 for jumps
    uniquenessRatio=10,                    # percent — the repetitive-texture guard
    disp12MaxDiff=1,                       # left-right consistency, in pixels
    speckleWindowSize=100, speckleRange=2)
disp = sgbm.compute(rectL, rectR).astype(np.float32) / 16.0   # output is 4 fractional bits
Z = f * B / np.where(disp > 0, disp, np.nan)

That / 16.0 is the single most common bug in stereo code written by people who did not read the docs: OpenCV returns disparity as a fixed-point int16 with four fractional bits, so forgetting it makes every depth sixteen times too small and everything looks like it is in your face.

From disparity to points, and the pixels that fly

Disparity is not a point cloud, and the step between them hides a failure that puts phantom obstacles directly in the robot's path.

The reprojection is a single matrix. For a rectified pair with the reference camera's intrinsics (f, c_x, c_y) and baseline B:

with Q written out row by row, so that nothing is hidden inside the word "matrix":

Worked example 5 — one pixel, every entry, then the same pixel with a calibration bias

Do this once by hand and you will never again have to trust that the library agrees with the geometry. Take the rig we have used throughout — f = 700 px, B = 0.12 m, c_x = 640, c_y = 360 — and one matched pixel: u = 800, v = 300, d = 42 px. The input vector is [u  v  d  1]^T = [800  300  42  1]^T. Multiply, one row at a time, every product visible:

Now the perspective divide, which is the step that turns four homogeneous numbers into three metres:

Check the depth against the relation we derived at the top of the chapter: Z = fB/d = 84.0/42.0 = 2.000 m. They agree exactly, which is the point — the matrix is not new physics, it is the same similar-triangles relation with the lateral coordinates carried along. Reading the rows back tells you why the last row is where the depth actually lives: W = d/B, so Z = Z_h/W = f/(d/B) = fB/d, and the other two follow with no extra work:

Sanity-check the first one on our pixel: (800 − 640) × 0.12/42 = 19.2/42 = 0.4571 m. Same answer, no matrix required — and now the error claim is algebra rather than assertion. All three coordinates carry the same factor 1/d, so a fractional error in disparity produces the same fractional error in X, Y and Z: σ_X/|X| = σ_Y/|Y| = σ_Z/Z = σ_d/d. The lateral position of a far point is exactly as uncertain, in relative terms, as its range, so the absolute lateral uncertainty grows with how far off axis the point is. At 10 m with σ_d = 0.2 px that fraction is 0.2/8.4 = 2.38%, so a point 10 m out and 5 m off to the side carries σ_X = 0.0238 × 5 m = 11.9 cm of lateral uncertainty alongside its 0.0238 × 10 m = 23.8 cm of range uncertainty. A point on the optical axis, by contrast, has X = 0 and therefore no lateral uncertainty at all from this source — the error ellipsoid is a needle pointing back at the camera, and it fans out as you move off axis.

The degraded twin. Now run the identical pixel through the identical matrix with the 0.02° relative yaw from Worked example 4, which adds a uniform δd = +0.2443 px to every match. Only the third input changes: d = 42.0000 → 42.2443 px. Three of the four rows do not involve d at all, so X_h, Y_h and Z_h are untouched at 160, −60 and 700. The fourth row moves:

and the whole point moves with it, because W is the common divisor:

That −11.6 mm is the same number the yaw table gave for 2 m (1.2 cm), reached by a completely different route — a good habit: when two derivations in the same chapter can be made to agree, make them agree out loud.

The structure of that change is worth more than the change itself. Every coordinate was divided by the same W, so the biased point is the clean point scaled by 42.0/42.2443 = 0.99422 — a pure radial contraction toward the camera centre, 0.58% at this range. That is why a fronto-parallel wall survives the bias looking perfect: every pixel on it shares one disparity, so every point contracts by one factor, and a plane maps to a plane. Fit a plane, get a beautiful fit, and conclude the depth is healthy.

But the contraction factor is d/(d + δd), which depends on range: 0.58% at 2 m and δd/(d + δd) = 0.2443/8.6443 = 2.83% at 10 m, nearly five times larger. So a surface that spans range — the floor, an aisle receding from the robot — is not contracted, it is bent. That gives you the second free online check to sit beside the vertical-disparity residual: fit a plane to the visible floor and plot the residual against range. Healthy rectification gives white scatter; a yaw bias gives a residual that grows monotonically with range and keeps one sign, with a 1.2 cm to 28.3 cm span across 2–10 m that no plane fit can absorb.

The flying pixel. A matching window that straddles a depth discontinuity contains texture from both surfaces, so the cost minimum lands somewhere between the two true disparities. Work a concrete case: a pallet edge at 2.0 m gives d = 84/2.0 = 42.0 px, and the aisle wall 8 m behind it gives d = 84/8.0 = 10.5 px. A window split evenly across the edge produces a compromise near d = 26.25 px, which reprojects to Z = 84/26.25 = 3.20 m.

That is a 3-D point suspended in mid-air — 1.2 m behind the pallet and 4.8 m in front of the wall — and it is in free space the robot might want to drive through. Every depth discontinuity in the scene produces a string of them, which is why raw stereo clouds look like they have been sprayed with confetti along every edge.

The mitigation is a disparity-gradient filter: reject any pixel whose matching window spans a disparity range larger than a threshold, since a physically real surface cannot be at two depths inside one window. The check costs a min-max over the window and it is the difference between a costmap you can plan on and one you cannot. It is also a good thing to volunteer, because it demonstrates you have looked at a real cloud rather than a benchmark's ground truth.

But what threshold? "Two pixels" is the answer people give, and it is wrong at one range and right at another, because the legitimate disparity gradient of a real surface depends on how far away it is. Derive it. Take a planar surface tilted by α from fronto-parallel. One pixel of image step corresponds to Z/f metres of lateral motion on the surface, so the depth changes by ΔZ = (Z/f)·tanα per pixel. Differentiating d = fB/Z gives |dd/dZ| = d/Z, so the disparity changes by

The Z cancels, and what is left says the legitimate gradient scales with the disparity itself. At 2 m (d = 42) a steeply slanted floor at α = 80° gives (42/700) × 5.671 = 0.34 px per pixel, so 1.7 px across a 5-px window. At 1 m (d = 84) the same physical slant gives 0.68 px per pixel — 3.4 px across the same window. A fixed 2 px threshold therefore keeps the slanted floor at 2 m and throws away the identical floor at 1 m. Rearranging the same equation removes the range dependence entirely:

so threshold on the implied slant angle, not on raw disparity gradient. Check it against the pallet edge: the window spans 31.5 px over 5 px of image, a gradient of 6.3 px/px, giving tanα = 6.3 × 700/42 = 105 and α = 89.45° — a surface almost exactly edge-on to the camera, which is precisely what a depth discontinuity pretends to be. Reject above ~80° and the flying pixels go while the real floor stays.

Both halves of the step — the reprojection and the filter — in numpy, with no library doing the thinking:

python
import numpy as np
from numpy.lib.stride_tricks import sliding_window_view

def reproject(disp, f, B, cx, cy, min_disp=1.0):
    # disp: float32 HxW in PIXELS (already divided by 16 if it came from OpenCV)
    H, W = disp.shape
    u, v = np.meshgrid(np.arange(W, dtype=np.float64), np.arange(H, dtype=np.float64))
    valid = disp > min_disp          # d -> 0 sends Z -> inf; guard BEFORE dividing
    d = np.where(valid, disp, np.nan)
    Z = f * B / d                    # row 3 over row 4 of Q
    X = (u - cx) * B / d             # row 1 over row 4
    Y = (v - cy) * B / d             # row 2 over row 4
    return np.stack([X, Y, Z], axis=-1), valid

def slant_mask(disp, f, w=5, max_slant_deg=80.0):
    # A real surface cannot be at two depths inside one window. Normalising the
    # gradient by d/f turns the threshold into a range-independent slant angle.
    win = sliding_window_view(disp, (w, w))            # (H-w+1, W-w+1, w, w)
    lo = np.nanmin(win, axis=(-2, -1))
    hi = np.nanmax(win, axis=(-2, -1))
    k = w // 2
    ctr = disp[k:k + lo.shape[0], k:k + lo.shape[1]]
    grad = (hi - lo) / w                               # px of disparity per px of image
    slant = np.degrees(np.arctan(grad * f / np.maximum(ctr, 1e-6)))
    keep = np.zeros_like(disp, dtype=bool)
    keep[k:k + lo.shape[0], k:k + lo.shape[1]] = slant < max_slant_deg
    return keep

Then the library form, and the trap inside it:

python
Q = np.float64([[1, 0, 0, -cx],
                [0, 1, 0, -cy],
                [0, 0, 0,   f],
                [0, 0, 1/B, 0]])
pts = cv2.reprojectImageTo3D(disp, Q, handleMissingValues=True)
# TRAP: missing values are not NaN — OpenCV writes Z = 10000.0 (ten kilometres).
pts = pts[np.isfinite(pts).all(-1) & (pts[..., 2] < 100.0)]

Three things to keep straight while writing that. First: "I guard the divide before I do it, because d = 0 is not an error condition in the data, it is the normal encoding of 'no match', and it produces infinities that survive all the way into a voxel grid." Second: "handleMissingValues writes 10000.0, not NaN, so an unfiltered cloud contains a wall of points ten kilometres away — which silently blows up any structure sized from the cloud's bounding box, and is the reason someone's octree allocated 40 GB." Third, the subtle one: "the Q that stereoRectify hands back has (c_x − c_x′)/T_x in the bottom-right corner rather than zero. It is zero only when the two rectified principal points agree; if it is not, my hand-written version and OpenCV's will disagree, and OpenCV is right."

Two failures, and the metric that separates them

Failure 1: perfect indoors, empty outdoors

Symptom. The depth camera returns a valid-pixel fraction of 0.94 in the warehouse and 0.11 in the loading yard at midday. It recovers at dusk. Someone suggests the exposure is wrong.

Root cause. The projected NIR pattern is at 0.92% contrast against filtered sunlight, as computed above. There is no exposure that fixes a signal buried under the shot noise of the background.

The metric that reveals it, and the decoy it rules out. Two candidate causes predict the same symptom: (a) the projector is swamped, (b) the sensor is saturating and losing all texture to clipping. They are separated by a thirty-second experiment: turn the projector off and re-measure the valid fraction.

• If the valid fraction is unchanged (0.11 with and without), the projector was contributing nothing — cause (a). Everything valid you are getting is from natural texture.
• If the valid fraction collapses further (0.11 → 0.02), the projector is still doing work and the problem is elsewhere — look at the histogram for clipping at 255.

The continuous version to put on a dashboard: log valid-pixel fraction against measured ambient irradiance, and against the fraction of pixels at the top of the histogram. Two curves, two causes, no argument.

Failure 2: a clean, confident, wrong plane

Symptom. In front of a rack of identical grey totes, the depth map shows a smooth surface at 3.1 m. The real rack is at 2.3 m. There is no noise, no speckle, no holes — it looks like the best depth in the whole frame. The robot plans through it.

Root cause. The totes repeat with a period of about 22 px in the image. The matcher found the minimum one period off, and because every pixel in the region found the same wrong minimum, the result is spatially consistent and survives the smoothness prior, the speckle filter, and often the left-right check as well (both directions can agree on the same periodic alias).

The metric that reveals it. The uniqueness ratio: second-best cost divided by best cost, over disparities more than one or two away from the winner. Healthy texture gives 1.5–3.0. Repetitive structure gives 1.00–1.05. Publish the per-pixel ratio as a confidence channel and this region lights up as the least trustworthy in the frame, even though every other quality signal calls it the best.

What is changing

Learned stereo replaced hand-built cost volumes. Lipson, Teed and Deng, "RAFT-Stereo: Multilevel Recurrent Field Transforms for Stereo Matching" (3DV 2021), carried the recurrent refinement idea from optical flow into stereo and pushed error rates well below SGM on standard benchmarks, particularly in the thin-structure and textureless regimes where classical matching is weakest. Wen et al., "FoundationStereo" (CVPR 2025), took the next step of training one model to generalise zero-shot across domains rather than fine-tuning per dataset. The exponent in σ_Z ∝ Z² does not change — that is geometry, not algorithms — but σ_d gets smaller and the invalid-pixel fraction drops a lot.

Monocular metric depth stopped being a joke. Yang et al., "Depth Anything V2" (NeurIPS 2024), and Piccinelli et al., "UniDepth: Universal Monocular Metric Depth Estimation" (CVPR 2024), produce dense metric depth from a single image with usable accuracy in familiar domains. The honest position: these are excellent priors and poor measurements. They fill the holes stereo leaves, they resolve which side of an edge is nearer, and they must never be the only evidence behind a safety decision, because their failure mode is confident-wrong on out-of-distribution scenes and they carry no geometric constraint that could contradict them.

The dated claim to watch. Anything stated here about which network holds a benchmark is perishable; the geometry is not. If you are asked about the frontier, lead with the invariant (quadratic depth error, occlusion band equal to the disparity, uniqueness as the only internal guard against repetition) and treat model names as illustrations. That framing survives the next release cycle and reads as judgement rather than currency.

Chapter 3: LiDAR — A Sweep Is Not a Snapshot

Chapter 0 covered what LiDAR is blind to. This chapter is about the failure that is always present, on every scan, on every moving robot, and that most people have never had to write down: a spinning LiDAR does not take a picture, it paints one, and while it paints the canvas moves.

The sweep as a trajectory

A 10 Hz spinning LiDAR completes one revolution in 100 ms. Points at azimuth 0° and points at azimuth 359° are separated by 100 ms of robot motion. Every driver hands you a single message with a single header timestamp, and if you believe that timestamp you have quietly asserted that the robot was frozen for a tenth of a second.

It was not. At a modest 1.5 m/s the robot translates 15 cm during one sweep, and at 60°/s of yaw it rotates 6°. Both of those are far larger than the sensor's own 2–3 cm range noise, so motion distortion dominates the error budget of a moving LiDAR and everything downstream inherits it.

The fix is to treat the sweep as what it is: a sequence of measurements taken from a sequence of poses. Choose a reference instant — conventionally the end of the sweep, because that is the pose you want to publish — and transform every point into that frame.

Every symbol. p_i is the point as the sensor measured it, expressed in the sensor frame at the instant of that firing — that is just (r cosα, r sinα) in 2-D, or the beam direction times the range in 3-D. ξ = (v, ω) is the body twist, assumed constant across the sweep. t_i − t_end is the time offset, and it is negative for every point except the last one. exp(·) is the SE(2) or SE(3) exponential, which turns a constant twist held for a duration into a rigid transform.

Where V(θ) comes from — deriving the one non-obvious object

"The SE(2) exponential" is easy to say and easy to hand-wave, and the hand-wave is exactly where understanding usually stops. So derive it. The rotation half is not the problem: hold yaw rate ω for a duration dt and you have turned by θ = ω·dt, full stop, no correction factor anywhere. The translation is the problem, and it is worth being precise about why.

The velocity v is a body-frame quantity — "1.5 m/s out the front of the robot". But the robot's front is turning while that 1.5 m/s accumulates. At the start of the interval the velocity points one way; a millisecond later it points somewhere slightly different. You cannot multiply a rotating vector by a duration. You have to integrate it, re-expressing the body velocity in the fixed frame at every instant along the way:

Here s is a dummy variable running over the interval, R(ωs) is how much the body has already turned by time s, and v is constant (that is the constant-twist assumption). Write the 2×2 product out component by component with v = (v_x, v_y):

Two elementary integrals appear, and only two. Both are first-year calculus:

Now substitute θ = ω·dt, which means 1/ω = dt/θ, and factor the dt out of both entries. What is left is a matrix acting on v, times dt:

That matrix is V(θ), the left Jacobian of SO(2). It is not a fudge factor, not an approximation, and not something you memorise: it is what the integral evaluated to. And it has a one-line interpretation that makes it stick — divide the integral by dt and you are averaging R(φ) over φ from 0 to θ:

The limit is the sanity check. Let θ → 0. Then sinθ/θ → 1 and (1−cosθ)/θ → θ/2 → 0, so V → I and Δp → v·dt — exactly the naive answer everyone writes first. So the naive form is not wrong; it is the zeroth-order truncation of this integral, valid when the body barely turned during the interval.

Better still, the series tells you the size of what you dropped. Using sinθ/θ = 1 − θ²/6 + … and (1−cosθ)/θ = θ/2 − θ³/24 + …, the error of the naive form is

The along-track term is second order in θ and the across-track term is first order. So the naive small-angle de-skew loses almost nothing in the direction of travel and everything it loses is lateral — which is worth knowing, because lateral is the direction that bends walls. Step 4 of the worked example below quotes a 2.0 mm discrepancy; that number is (θ/2)·|v·dt| = 0.026180 × 0.075 = 1.963 mm of lateral offset plus a mere (θ²/6)·|v·dt| = 0.000457 × 0.075 = 0.034 mm along track. Now you know exactly where the 2 mm went.

Worked example 1 — how bent, exactly

Robot driving forward at v = 1.5 m/s while yawing at ω = 60°/s = 1.047198 rad/s. The LiDAR spins at 10 Hz, so t_end − t_start = 0.100 s. Take the beam that fires exactly halfway through the sweep, at t_i − t_end = −0.050 s, and suppose it measures a range of 10.000 m straight ahead in the sensor frame at that instant, so p_i = (10.000, 0.000).

1. Rotation over the offset: Δθ = ω · dt = 1.047198 × (−0.050) = −0.052360 rad = −3.000°.
2. Rotation matrix: R = [[cos(−3°), −sin(−3°)], [sin(−3°), cos(−3°)]] = [[0.998630, 0.052336], [−0.052336, 0.998630]].
3. Rotate the point: R · (10, 0) = (9.986295, −0.523360).
4. Translation — and this is the step people skip. Δp = V(Δθ) · v · dt, where V is the left Jacobian derived above. It is two scalars, so compute them at θ = −0.052360 rad:
   1. A = sinθ/θ. sin(−0.052360) = −0.05233598, so A = (−0.05233598)/(−0.05235990) = 0.999543. (Just under 1, as the chord-vs-arc argument said it must be.)
   2. B = (1 − cosθ)/θ. cos(−0.052360) = 0.99862953, so the numerator is 1 − 0.99862953 = 0.00137047, and B = 0.00137047/(−0.05235990) = −0.026174. B is negative because θ is; the numerator 1 − cosθ is positive for either sign of θ, so B carries the sign of the turn. Sanity check against the series: θ/2 = −0.026180, which agrees to four decimals.
   3. Assemble V = A·I + B·[[0,−1],[1,0]] = [[A, −B], [B, A]] = [[0.999543,  +0.026174], [−0.026174,  0.999543]].
   4. The vector it multiplies is v·dt = (1.5, 0.0) × (−0.050) = (−0.075000, 0.000000). So
      Δp_x = 0.999543 × (−0.075000) + 0.026174 × 0.000000 = −0.074966 m
      Δp_y = (−0.026174) × (−0.075000) + 0.999543 × 0.000000 = +0.001963 m
   The naive small-angle version, v·dt = (−0.075000, 0.000000), is off by 2.0 mm — and by the expansion above that error is 1.963 mm lateral plus 0.034 mm along track. Negligible here, at 3° of turn; at 30° the lateral term is (θ/2)·|v·dt| with θ = 0.5236, which is 26% of the displacement and no longer negligible at all. Worth spelling out, because knowing where the approximation lives is knowing when it breaks.
5. Corrected point: (9.986295 − 0.074966, −0.523360 + 0.001963) = (9.911330, −0.521397).

So a point the raw cloud places at (10.000, 0.000) actually belongs at (9.911, −0.521). The correction is 52.9 cm, of which 52.1 cm is lateral. Now sweep the offset across the whole revolution and the picture becomes unmistakable:

A flat wall 10 m away becomes a metre-long smear, monotonically increasing with how early in the sweep the beam fired. Against a sensor whose own range noise is 2 cm, the distortion is fifty times the noise. Saying "LiDAR is accurate to a couple of centimetres" and stopping there is the tell of someone who has never processed a moving scan.

Spinning, solid-state, and what changes

"Spinning versus solid-state" is a stock debate, and the stock answer — "no moving parts, more reliable" — is worth almost nothing. The properties that actually change your software are these:

The line that summarises it: "The mechanically interesting difference is the moving parts; the software-interesting difference is that a non-repetitive scan pattern destroys the assumption that azimuth index is a proxy for time and that neighbouring points are neighbours in space. Anything I wrote that indexes into a ring has to be rewritten."

Worked example 2 — beam divergence and the mixed pixel

The beam is not a line. A typical divergence is about 3 milliradians, so the illuminated spot has diameter 0.003 × R — at 30 m that is 0.003 × 30 = 0.090 m, which is the third row below and every other row is the same single multiplication:

When the spot straddles a depth discontinuity — half on a pole at 30 m and half on a wall at 45 m — the receiver sees two echoes inside one measurement. A sensor that reports the amplitude-weighted first moment of that merged waveform places a mixed pixel at an intermediate range that corresponds to nothing physical. These are the "flying pixels" that pepper every real cloud along object edges. The right move is to compute where it lands rather than gesture at "somewhere in between", so here is the whole calculation.

Step 1 — the return model, stated explicitly. For a surface that fills the beam, received power goes as

Every symbol. ρ is the diffuse (Lambertian) reflectivity of the surface. f is the fraction of the beam's cross-section that lands on it, so the two surfaces' f values sum to 1. R is range. The exponent deserves a sentence, because "why R² and not R⁴?" is a real follow-up: the receiver collects a solid angle A_r/R² of the reflected light, which alone would give 1/R⁴ — but the illuminated area also grows as R² for a diverging beam on an extended surface, and more illuminated area means more reflecting area. Those two R² factors cancel one against the other, leaving 1/R² for a beam-filling target. (A target smaller than the beam does not grow with range, so it keeps the full 1/R⁴. That distinction is why thin wires and small retroreflectors behave so differently at range.)

Step 2 — evaluate both returns. Take a bare concrete pole, ρ = 0.3, at R = 30 m, with the spot half on it; and a painted wall, ρ = 0.5, at R = 45 m, taking the other half. Constants of proportionality cancel in the ratio, so drop them:

1. I_pole = f·ρ/R² = 0.5 × 0.3 / 30² = 0.15 / 900 = 1.667 × 10⁻⁴
2. I_wall = 0.5 × 0.5 / 45² = 0.25 / 2025 = 1.235 × 10⁻⁴
3. Sum: I_pole + I_wall = 1.667e−4 + 1.235e−4 = 2.902 × 10⁻⁴

Notice the competition before you go further: the wall is 1.67× more reflective, but it is 1.5× further, and 1.5² = 2.25 beats 1.67. The near, darker surface wins. That is the whole reason the phantom does not land at the midpoint.

Step 3 — the intensity-weighted centroid. The reported range is the first moment of the merged return:

1. Numerator term 1: 1.667e−4 × 30 = 5.000 × 10⁻³
2. Numerator term 2: 1.235e−4 × 45 = 5.556 × 10⁻³
3. Numerator: 5.000e−3 + 5.556e−3 = 1.0556 × 10⁻²
4. R_mix = 1.0556e−2 / 2.902e−4 = 36.4 m

So the phantom sits at 36.4 m, not at the 37.5 m midpoint — pulled 1.1 m toward the pole by the inverse-square weighting, exactly as step 2 predicted. It floats 6.4 m past the pole and 8.6 m short of the wall, in open air, and the spot that produced it is 0.003 × 36.4 = 10.9 cm across. Nothing in the point's own record — range, intensity, ring, timestamp — says anything is wrong.

Two consequences you should volunteer. First, dual-return mode exists for exactly this reason: reporting both the strongest and the last echo lets you keep the far surface and identify the edge. Second, a mixed pixel is a confident-wrong measurement with normal intensity, so no per-point quality metric catches it — the standard defence is a neighbourhood consistency filter that removes points whose range differs sharply from both azimuthal neighbours.

Where LiDAR dies in weather

Chapter 0 covered material failures. Weather is the other axis, and it has two distinct mechanisms that people conflate.

Attenuation follows Beer-Lambert over the two-way path: transmission = exp(−2αR), where α is the extinction coefficient in km⁻¹ and R is the range in km. With α ≈ 1 km⁻¹ in heavy rain, a 50 m target retains exp(−2 × 1 × 0.050) = exp(−0.1) = 0.905 of its energy — a 9.5% loss. Rain is a surprisingly minor attenuator.

Fog is not. To talk about fog in numbers you need the bridge between the extinction coefficient and the thing the weather report gives you, which is Koschmieder's relation: meteorological visibility V is the range at which contrast falls to the 2% threshold, and that works out to

So α ≈ 20 km⁻¹ is not a 50 m fog — it is 3.912/20 = 0.196 km, a 196 m fog, the kind you would drive through with fog lights on. Even that costs the LiDAR dearly: the same 50 m target retains exp(−2 × 20 × 0.050) = exp(−2.0) = 0.135, an 87% loss, which takes most low-reflectivity targets under the detection floor. Now tighten to a genuine 50 m visibility: α = 3.912/0.050 = 78 km⁻¹, and the 50 m target retains exp(−2 × 78 × 0.050) = exp(−7.8) ≈ 4 × 10⁻⁴. Four parts in ten thousand. The target is simply gone, and no amount of gain helps because the noise floor came up with it.

Backscatter is worse and more interesting. Droplets in the beam path return energy from ranges closer than the target. A leading-edge detector triggers on the first thing above threshold, so the sensor reports a phantom point a few metres ahead in open air. The observable signature is distinctive: a diffuse halo of low-intensity points at short range that moves with the robot rather than staying fixed in the world, and that vanishes from the last-return channel. That is why last-return mode is the standard rain setting — the raindrops are the first echo, the road is the last one.

What to look for. Press "flip the dt sign". The walls become perfectly straight again — and the whole room shifts, because you have de-skewed into the sweep-start frame instead of the sweep-end frame. That is the demonstration that straightness is not a correctness test. The only correctness test is whether the cloud registers against a map built in the same convention, which is why this bug survives code review and dies in the field.

The LiDAR path, with numbers

The ordering constraint that gets violated in real pipelines. De-skew must come before voxel downsampling and before any feature extraction. Downsampling a distorted cloud averages points that were taken from different poses, so it bakes the distortion in irreversibly and it is not even a consistent average — the bias depends on how many early-sweep versus late-sweep points landed in each voxel. Put de-skew immediately after assembly and before everything else.

The dependency that makes it a design problem. De-skew needs to know the motion during the sweep, which means it needs a pose source that spans the sweep interval. That rules out the output of the very estimator that consumes the cloud — you would be using the answer to compute the question. The three real options, and the one you should pick:

The budget, stated in numbers. Latency from first firing to a registered pose: 100 ms of sweep accumulation (unavoidable, and it is why the effective LiDAR latency is half a sweep even before processing) + 2 ms assembly + < 1 ms de-skew + 4 ms filtering + 20 ms registration = ~127 ms, of which 79% is waiting for the sensor to finish turning. That is the number that motivates processing sub-sweep slices at 40 Hz instead of full revolutions at 10 Hz, and being able to say it is what separates someone who has tuned a stack from someone who has read about one.

De-skew from scratch, then the library form

python
import numpy as np

def se2_exp(v, w, s):
    # the pose reached by holding twist (v, w) for time s, as (theta, translation)
    th = w * s
    if abs(th) < 1e-9:
        return th, np.array([v[0] * s, v[1] * s])
    A, Bc = np.sin(th) / th, (1 - np.cos(th)) / th
    V = np.array([[A, -Bc], [Bc, A]])
    return th, V @ (np.asarray(v) * s)

def deskew(rng, az, t, t_end, v, w):
    # rng, az, t are per-point. Returns the cloud in the SWEEP-END frame.
    out = np.empty((len(rng), 2))
    for i in range(len(rng)):
        dt = t[i] - t_end               # NEGATIVE for every point but the last
        th, dp = se2_exp(v, w, dt)
        R = np.array([[np.cos(th), -np.sin(th)],
                      [np.sin(th),  np.cos(th)]])
        p = np.array([rng[i] * np.cos(az[i]), rng[i] * np.sin(az[i])])
        out[i] = R @ p + dp
    return out

The vectorised production form does the same arithmetic without the Python loop, and in practice you interpolate a real IMU-derived trajectory rather than assuming one constant twist:

python
# vectorised, constant twist — this is what runs at 10 Hz
dt  = t - t_end
th  = w * dt
c, s = np.cos(th), np.sin(th)
px, py = rng * np.cos(az), rng * np.sin(az)
A  = np.where(np.abs(th) < 1e-9, 1.0, np.sin(th) / np.where(th == 0, 1, th))
Bc = np.where(np.abs(th) < 1e-9, 0.0, (1 - c) / np.where(th == 0, 1, th))
X = c * px - s * py + (A * v[0] - Bc * v[1]) * dt
Y = s * px + c * py + (Bc * v[0] + A * v[1]) * dt

# in a real stack you do not assume constant twist — you SLERP the IMU trajectory:
#   pose_i = interpolate(imu_poses, t[i])    # 200 Hz IMU spans the 100 ms sweep
#   p_end  = T_end.inverse() * pose_i * p_i

Two failures, and the metric that separates them

Failure 1: double walls, but only at corners

Symptom. The occupancy map is crisp along straight aisles and shows two parallel walls 15–20 cm apart wherever the robot turned. Straight-line sections are fine. The effect is worse for far walls than near ones.

Root cause. Motion distortion during the sweep, uncorrected. Registration finds a compromise pose between the early-sweep and late-sweep halves of the smeared cloud, and consecutive smeared scans disagree, so the map accumulates both.

The metric that reveals it, and the three decoys it beats. Fit a plane (or line) to a known flat wall in each scan and record the RMS residual. Then scatter that residual against |ω| for every scan in the log. The four candidate causes make four different predictions, and this one plot separates them all:

Motion distortion and a time offset both scale with rate and both vanish at rest — so one more test is needed, and it is decisive: de-skew the log offline and re-plot. If the slope goes to zero, it was distortion. If a slope remains, you have a residual time offset, and the offset in seconds is that remaining slope divided by the range at which you measured it. This is the same "divide the bias by the rate to get seconds" move from Chapter 1, and it is worth noticing that it keeps working.

Failure 2: de-skew made it worse

Symptom. Somebody enables de-skew. Straight walls are now beautifully straight. Localisation error doubles, and the robot consistently believes it is about 15 cm behind where it is.

Root cause. The reference instant. The de-skew used dt = t_i rather than t_i − t_end, producing a cloud that is internally consistent in the sweep-start frame while the published pose timestamp says sweep end. The result is a fixed lag of one sweep period.

The metric that reveals it. Not scan quality — scan quality got better, which is exactly why this bug ships. The tell is that the position error is a constant offset along the direction of travel whose magnitude equals speed × sweep period: 1.5 × 0.100 = 15 cm, and it scales with speed while staying stationary in the body frame. Plot the localisation residual in the body frame rather than the world frame and it collapses to a single point instead of a cloud. A calibration error would also give a constant body-frame offset, but it would not scale with speed; divide by speed and if you get a number equal to your sweep period, you have found it.

What is changing

FMCW measures velocity per point. Frequency-modulated continuous-wave LiDAR mixes the return with a local copy of the transmitted chirp and reads range from the beat frequency and radial velocity from the Doppler shift on the very same measurement. That changes this chapter structurally: with a per-point radial velocity you can segment moving objects from a single sweep with no association or tracking, and you get a direct observation of the ego-motion component that de-skew needs. Behroozpour et al., "Lidar System Architectures and Circuits" (IEEE Communications Magazine, 2017), is the readable architectural reference. FMCW is also immune to interference from other LiDARs and to sunlight, because only a return coherent with your own chirp produces a beat.

Continuous-time registration made de-skew part of the estimate. Dellenbach, Deschaud, Jacquet and Goulette, "CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure" (ICRA 2022), stopped treating de-skew as a preprocessing step with an externally supplied twist and instead parameterised each scan by its start and end poses, optimising both inside the registration. The scan is allowed to be elastic. This removes the circular dependency in the design table above — you no longer need a twist before you can register — and it is the single best answer to "how would you do de-skew today".

The convergence worth naming. Rolling-shutter cameras and spinning LiDARs have exactly the same problem: a measurement spread over time, modelled as if it were instantaneous. Both are solved the same way, by replacing "one pose per frame" with a continuous-time trajectory that the estimator can query at any timestamp. Noticing that — that Chapter 1's rolling shutter and Chapter 3's motion distortion are one problem — is the kind of connection that separates knowing the material from seeing the structure.

Chapter 4: IMU — Three Error Sources, Three Different Exponents

An IMU is the only sensor on the robot that never loses lock, never gets occluded, never cares about weather or texture or darkness, and runs at 200 Hz for a few dollars. It is also the only one whose error grows without bound in the absence of anything else. Both halves of that sentence are why every modern estimator is built around it.

Deriving the exponents under time pressure

The question is always some version of: "How far do you drift on IMU alone?" The wrong answer is a number. The right answer is three numbers with three different exponents, because which one dominates depends entirely on how long you have been drifting.

Source 1 — accelerometer bias, giving t²

An accelerometer bias b_a is a constant offset added to every reading. The navigator integrates it twice, so

With a modest b_a = 0.02 m/s² (about 2 milli-g, entirely typical of an uncalibrated consumer MEMS part): at t = 1 s the error is 0.5 × 0.02 × 1 = 1.0 cm. At t = 10 s it is 0.5 × 0.02 × 100 = 1.00 m. At t = 60 s it is 0.5 × 0.02 × 3600 = 36.0 m.

Source 2 — gyro bias, giving t³ (this is the one they want)

A gyro bias does not enter position directly. It enters through gravity, and the chain of reasoning is the single most valuable derivation in inertial navigation.

1. The gyro bias b_g is integrated into the attitude estimate, so after time t the navigator believes the body is tilted by θ(t) = b_gt when in fact it is level.
2. The accelerometer, being level and stationary, honestly reports a specific force of magnitude g pointing straight up in the body frame.
3. The navigator rotates that body-frame vector into the navigation frame using its wrong attitude, so it lands tilted by θ. Its horizontal component is g·sin(θ) ≈ g·b_gt.
4. The navigator then subtracts the known gravity vector (0, 0, −g), which cancels the vertical part and leaves the horizontal part untouched. What remains is a phantom horizontal acceleration a(t) = g·b_gt — growing linearly with time.
5. Integrate that twice: ∫∫ g b_gt dt dt = g b_g t³ / 6.

Work it numerically. b_g = 0.01°/s (a decent consumer-grade in-run stability) = 0.01 × π/180 = 1.745329 × 10^-4 rad/s.

Sixty-one metres in one minute, from a bias of one hundredth of a degree per second. That number is the answer to "why can't we just use the IMU in the tunnel", and it is worth committing to memory.

The crossover — which bias dominates, and when

Set the two equal and solve. ½b_at² = (1/6)g b_gt³ gives t = 3b_a / (g b_g). With b_a = 0.02 and b_g = 1.745e−4:

At that instant both terms equal 12.28 m. Below 35 seconds the accelerometer bias is the bigger problem; above it the gyro takes over and never gives it back, because a cubic beats a quadratic forever. This is precisely why a robot that gets a GPS or LiDAR fix every second or two can live with a cheap gyro, and a robot that must coast for a minute cannot.

Source 3 — white noise, giving t^1.5

Bias is a systematic error; noise is a random one, and it grows more slowly. Model the accelerometer's white noise by its velocity random walk N, in units of m/s per √s — the standard deviation of the velocity error you accumulate in one second of integration.

Integrating white noise once gives a random walk with variance N²t — that is the definition of N. Integrating twice takes four more lines, and they are worth owning, because the factor of 1/3 is exactly where hand-waving hides. Reciting "standard Wiener result" is where the point is lost.

1. Position is the integral of velocity: p(t) = ∫₀^t v(s) ds, where v is a Wiener process with Var[v(s)] = N²s.
2. The variance of an integral is the double integral of the covariance — you cannot pull Var through ∫ the way you pull E through it, because the integrand at different times is correlated: Var[p(t)] = ∫₀^t∫₀^t Cov(v(s), v(u)) ds du.
3. For a Wiener process, Cov(v(s), v(u)) = N²·min(s, u). The reason is mechanical: v(s) and v(u) are sums of independent increments, and the only increments they share are the ones before min(s, u), so they share exactly min(s, u) seconds of history and nothing else.
4. Split the square [0,t]×[0,t] along its diagonal. On the lower triangle s < u, so min(s,u) = s and ∫₀^t∫₀^u s ds du = ∫₀^t u²/2 du = t³/6. The upper triangle is the same integral with the labels swapped, so it also gives t³/6.
5. Add the two triangles: t³/6 + t³/6 = t³/3. That is where the 3 comes from — it is two halves of a square, not a fitted constant.

The discrete sanity check — and it is literally what experiment B in the Code Lab below measures. Step at interval dt. Sample j contributes a velocity kick a_jdt whose variance is N²dt (that is the density-to-per-sample conversion sig_a = VRW / sqrt(DT) you will see in the starter). Once delivered, that kick rides in the velocity for every remaining step, so by time t it has displaced the robot by (t − j·dt) × (a_jdt). The kicks are independent, so variances add:

Same answer by a different route, and this one makes the mechanism visible: early noise hurts quadratically more than late noise, because an early velocity error has the entire remaining interval to accumulate into position. A noise sample at t = 0 gets the full lever arm t; one at t = 59 s of a 60 s run gets a lever arm of 1 s and contributes 3600× less variance. Hold that picture and the √3 stops being a memorised constant: it is the price of averaging a lever arm that shrinks linearly to zero.

Datasheets usually quote VRW in m/s per √hour. Convert: 0.1 m/s/√hr ÷ √3600 = 0.1/60 = 0.001667 m/s/√s. Then

The three exponents in one line, ordered by how long they take to matter: white noise t^1.5, accel bias t², gyro bias t³. There is a fourth — angle random walk in the gyro, which enters attitude as √t and therefore position as t^2.5 — and mentioning it unprompted is a nice touch, though it rarely dominates.

Reading an Allan deviation plot in fifteen seconds

The Allan deviation plot is a log-log graph of σ(τ) against averaging time τ, and the whole point is that each error source has its own slope, so you read the parameters straight off the curve.

All three recipes are the same trick, and saying so is faster than memorising three rules: each error source obeys a power law σ(τ) = C · τ^slope with a known constant folded in, so you evaluate the law at the one τ where that constant becomes 1 and read the coefficient straight off the plot. White noise obeys σ(τ) = N/√τ, so at τ = 1 s, σ = N with no scaling at all. Rate random walk obeys σ(τ) = K√(τ/3), so at τ = 3 s, σ = K. The flat region is the odd one out only because its constant, 0.664, is not 1 at any τ — it is a property of the flicker spectrum rather than of where you stand on the axis, so it never cancels and you always divide by it.

Worked, the +1/2 branch. A gyro whose curve passes through σ = 0.004°/s at τ = 300 s on the rising side has

Round-trip it before you quote it, which takes three seconds and catches the commonest mistake. Walk that same +1/2 line back down from τ = 300 s to τ = 3 s: τ falls by 100×, and on a +1/2 slope σ falls by √100 = 10×, giving σ(3 s) = 0.0004°/s — which is K, as the recipe promised. If your K does not survive that round trip you read a point off the wrong branch of the bowl, and on a real Allan plot the two branches meet in a broad flat minimum where it is genuinely easy to do so. The defensive habit: only read a coefficient from a τ at least a decade away from σ_min, where the slope is unambiguously ±1/2.

Worked: a gyro's Allan deviation bottoms out at σ_min = 0.002°/s at τ = 100 s. Then the bias instability is B = 0.002 / 0.664 = 0.00301°/s = 10.8°/hr. That single number tells you which grade of device you are holding:

Every cell in that table is one formula, so regenerate it rather than memorise it. Each entry is p(t) = g·b_gt³/6 with b_g converted out of °/hr by multiplying by π/(180 × 3600) = 4.8481 × 10^-6 rad/s per °/hr. Do the last row by hand: 0.01 × 4.8481e−6 = 4.8481e−8 rad/s; times 9.81 = 4.756e−7; times t³/6 = 1000/6 = 166.67 gives 7.93e−5 m = 0.079 mm, and times 216000/6 = 36000 gives 0.01712 m = 1.71 cm.

Because p is linear in b_g, the position column is a clean image of the specification column: consumer to navigation is 30 / 0.01 = 3000× in bias stability and therefore exactly 3000× in position error — 0.23780 m ÷ 0.079267 mm = 3000.0. The rounded display values give 3030 only because 0.24 is itself rounded. Nothing in this table is approximate except the grade labels themselves, which is worth saying out loud, because it means you can invert it: a coasting requirement in metres maps one-to-one onto a line item in the bill of materials, and the mapping is a single multiplication.

The engineering move: given a coasting requirement, invert the cubic and name the grade. "You want under 20 cm after 30 seconds of tunnel? p = g b_gt³/6 gives b_g = 6 × 0.20 / (9.81 × 27000) = 4.5e−6 rad/s = 0.94°/hr — that is tactical grade, five figures, and it changes the bill of materials. Or you find an aiding source, which is what Chapter 6 is about."

What to look for. Drag the gyro bias down to navigation grade (0.01°/hr) and the cubic line drops below the others across the whole plotted range — that is what a hundred thousand dollars of ring-laser gyro buys. Now push the VRW up: the noise line moves but its slope never changes. Nothing you buy changes an exponent. The only thing that changes an exponent is adding a different sensor, which resets the clock.

Wiring an IMU, and the one real tradeoff

The IMU is trivially cheap in bandwidth and the least trivial in timing. Its whole design story is about when, not how much.

The tradeoff, in numbers. Reading the IMU one sample at a time costs an interrupt every 5 ms — 200 per second, each with its own hardware-accurate timestamp. Using the on-chip FIFO with a watermark of 10 samples costs 20 interrupts per second, a 20× reduction in CPU overhead, and adds 10 × 5 ms = 50 ms of latency to the newest sample plus the obligation to back-date the older nine by assuming a perfectly regular sample interval. On a 200 Hz control loop that 50 ms is ten control periods, which is unacceptable; on a 10 Hz mapping pipeline it is invisible. The right answer depends on the fastest consumer, and saying so is the answer.

The watermark ladder — the timing budget you whiteboard

Do not hand-wave "some latency". At 200 Hz the sample period is T = 5 ms. A FIFO that raises its interrupt when W samples are queued hands you a batch in which the newest sample is fresh and the oldest has been sitting in silicon for (W − 1)·T. So there are two numbers, and you need both:

The mean is the average over the batch of (W − 1 − k)·T, which is the arithmetic series (W − 1)/2. The worst case is the oldest sample's in-FIFO age, (W − 1)·T, plus one more period for the ISR to be scheduled and the burst read to complete — round it to W·T and you are safely conservative. Tabulate it against the two consumers that actually exist on this robot:

Read the last two columns and the architecture answers itself. There is no single W that serves both consumers: the control loop is only feedable by W = 1, and the mapper is comfortable anywhere up to W = 10. So the design is not a compromise value of W, it is two paths off one device — a per-sample interrupt path for control and attitude, and a batched path for mapping, logging and the vibration monitor. Proposing that split unprompted is the move; proposing "W = 8, it's a good middle ground" is the answer that fails, because it is the one value that satisfies nobody.

Notice what never entered the argument: bandwidth. Twelve bytes at 200 Hz is 2.4 kB/s whatever W is, and a W = 10 burst is 120 B, which at 10 MHz SPI is 120 × 8 / 10⁷ = 96 µs of transfer. Even at W = 32 the burst is 384 B and 307 µs. The IMU is a latency budget and an interrupt budget, never a bandwidth budget — the opposite of the camera in Chapter 1 and the LiDAR in Chapter 3, and naming that inversion is worth a sentence.

Back-dating the batch, and the error that buys you

The hardware gives you exactly one trustworthy timestamp per batch, taken when the watermark fired. Every other sample's time is reconstructed:

That reconstruction quietly assumes the device's sample interval is exactly T. It is not. The IMU runs on its own oscillator, typically specified to ±1–2%, and it is not disciplined to your host clock. At 1% the true interval is 5.05 ms rather than 5.00 ms, so each back-dated step accrues 0.05 ms of error and the oldest sample in a W = 10 batch is mis-stamped by

Is 0.45 ms bad? The only way to know is to multiply it by whatever you are integrating. At a yaw rate of 1 rad/s it is 0.45 mrad = 0.026° of attitude — below the tilt errors in the table at the top of this chapter. At 1.5 m/s it is 0.7 mm of position. Both are under the noise floor, so W = 10 is safe on this count. Now run the same sum at W = 100: 99 × 0.05 ms = 4.95 ms, which is 5 mrad (0.28°) and 7.5 mm — and, crucially, that error is systematic. It has the same sign in every batch, so it integrates rather than averaging away, exactly like the wheel-radius error in Chapter 5. The watermark is bounded by the clock-drift budget as well as by the latency budget, and the second bound bites long before most people expect.

The senior answer is not a bigger safety factor on T, it is a measured T: regress hardware timestamps against sample index over a few minutes and estimate the device's true mean interval as a slowly-varying state, exactly the offset-estimation machinery in Time & Sync. A 1% error on a nominal 5 ms is 3 µs of slope per sample; over 60 s of samples that is a residual you can fit to well under 0.1%, which shrinks the W = 100 penalty from 4.95 ms to under 0.5 ms without touching the hardware.

The per-consumer data contract

Boxes and arrows are not a design. Every arrow leaving the driver carries a declared type, shape, rate, frame and unit — and if you cannot state them, you have not designed the interface, you have drawn it:

Unpack the preintegration row, because that is the one they follow up on. The measurement itself is 9-dimensional — a relative rotation ΔR, a relative velocity Δv and a relative position Δp between the two keyframes. It is carried inside the estimator's standard 15-dimensional error-state block [δp, δv, δθ, δb_a, δb_g] so that the Jacobians ∂Δ/∂b are available and the whole integral can be corrected to first order when the filter's bias estimate moves, instead of being recomputed from raw samples. 9 + 6 = 15; that decomposition is the answer to "why 15 and not 9".

Now count the bytes both ways, because the intuition most people carry is wrong. At 30 Hz keyframing, N ≈ 6.7, so the input is 7 × 7 × 8 = 392 B and the output is (15 + 225) × 8 = 1920 B — preintegration makes the message roughly five times bigger. At 2 Hz keyframing, N = 100, the input is 5600 B and the output is still 1920 B, so now it compresses about 3×. The win was never bytes; it is states in the optimisation. Those N samples become zero extra nodes in the factor graph instead of N, and a graph whose node count is set by keyframes rather than by IMU samples is the difference between a solvable problem and an intractable one. Being able to say that with the byte counts in hand, and to name the regime where the message actually grows, is the difference between having read about preintegration and having budgeted for it.

The other timing detail that bites. The on-chip low-pass filter has group delay — typically a few milliseconds, and it changes when you change the filter bandwidth. If you tune the bandwidth down to reduce noise, you have silently added a time offset to every IMU sample and shifted your camera-IMU extrinsic estimate. That is a genuine field bug and it belongs in the same family as the rolling-shutter offset from Chapter 1: a bias proportional to rate, hiding in a geometry parameter. See Time & Sync for how to estimate it rather than guess it.

Preintegration, in one paragraph. Between two consecutive 30 Hz camera frames there are 200/30 ≈ 6.7 IMU samples. Naively, adding those to a factor graph adds 6.7 states per frame. IMU preintegration compresses them into a single relative-motion constraint plus its covariance, computed once in the body frame so it does not have to be recomputed when the estimator's bias estimate changes — instead a first-order correction is applied. The result is a graph with one node per keyframe instead of one per IMU sample, which is the difference between a solvable problem and an intractable one. The mechanics live in Modern VIO; what you need here is the reason it exists.

The strapdown loop and the Allan deviation

The 1-D version of strapdown that shows exactly where the t³ comes from:

python
import numpy as np
g = 9.81

def strapdown_1d(gyro, fx_body, fy_body, dt):
    # gyro: measured rate. fx/fy: measured specific force in the BODY frame.
    th = vx = px = 0.0
    for k in range(len(gyro)):
        th += gyro[k] * dt                       # attitude first — everything depends on it
        ax = np.cos(th) * fx_body[k] - np.sin(th) * fy_body[k]
        # nav-frame gravity is (0, -g): no horizontal component to subtract,
        # so the ENTIRE horizontal error above comes from the wrong theta
        vx += ax * dt
        px += vx * dt
    return px

Run that with gyro = b_g constant, fx_body = 0, fy_body = g and it reproduces g·b_gt³/6 to within the Euler discretisation error. That is the entire derivation, executable in eight lines, and being able to type it while explaining it is worth more than any amount of recitation.

The overlapping Allan deviation, which you compute from a two-hour static log:

python
def allan_dev(x, fs, taus):
    # x: static gyro or accel samples. Overlapping estimator — lower variance
    # of the estimate than the non-overlapping one, for free.
    theta = np.cumsum(x) / fs                     # integrate to the "angle" series
    out = []
    for tau in taus:
        m = int(round(tau * fs))
        if m < 1 or 2 * m >= len(theta): out.append(np.nan); continue
        d = theta[2*m:] - 2 * theta[m:-m] + theta[:-2*m]
        out.append(np.sqrt(np.mean(d ** 2) / (2 * tau ** 2)))
    return np.array(out)

# the library one-liner, if you are allowed one:
#   import allantools; t2, ad, _, _ = allantools.oadev(x, rate=fs, data_type='freq')

Three practical notes to volunteer. The log must be at least ten times the longest τ you intend to read, so a bias-instability floor at τ = 100 s needs a 1000 s log minimum and two hours is the usual recommendation. The device must be genuinely still and thermally settled, because a temperature ramp shows up as a spurious +1 slope that people misread as rate random walk. And the resulting numbers describe that unit at that temperature — switch-on-to-switch-on bias repeatability is a different specification entirely, and it is usually much worse.

Two failures, and the metric that separates them

Failure 1: fine on the bench, five times worse on the robot

Symptom. An IMU characterised on a granite block shows an Allan bias instability of 8°/hr. Bolted to the robot with the drive motors running, the effective bias is 40°/hr, and it is worse at some speeds than others. Nobody has changed the firmware.

Root cause: vibration rectification error. The accelerometer's response is not perfectly linear, so a zero-mean vibration does not average to zero — the quadratic term in its response rectifies the vibration into a DC offset. The specification is quoted in units of mg per g² of RMS vibration. Aliasing makes it worse: any vibration above half the output rate folds down into your band and cannot be filtered out afterwards, because it is already indistinguishable from real motion by the time you see it.

The metric that reveals it, and the decoy it beats. Two candidate causes predict "worse on the robot": vibration rectification and self-heating from the surrounding electronics. Distinguish them with two plots:

• Bias against RMS vibration level. Log the accelerometer's high-frequency RMS alongside the estimated bias. Rectification gives a clean quadratic relationship — bias ∝ (vibration RMS)² — and it responds within one sample when the motors stop.
• Bias against die temperature. Self-heating gives a bias that tracks temperature with a thermal time constant of tens of seconds, so it lags the motor state rather than following it instantly.

Turn the motors off and watch: an instant recovery is rectification, a slow exponential decay is thermal. The fixes are also different — rectification needs mechanical isolation and an anti-alias filter before the ADC, thermal needs a temperature model or a better thermal path. Prescribing the wrong one wastes a hardware revision.

Failure 2: the calibration is perfect and every boot is different

Symptom. A careful bias calibration is stored in flash. Some days the robot's initial heading is off by 2°, other days it is fine. Re-running the calibration always produces a good result — for that session.

Root cause. Confusing two specifications that live in different columns of the datasheet. In-run bias stability (what the Allan floor measures) describes how much the bias wanders during one power-up. Switch-on-to-switch-on repeatability describes how much it changes between power-ups, and for consumer MEMS parts it is often five to twenty times worse. A stored calibration corrects a bias the device no longer has.

The metric that reveals it. Power-cycle the unit fifty times, take a 60-second stationary average each time, and plot the histogram of those averages. In-run stability predicts a tight cluster; switch-on repeatability shows up as a spread across boots that is far wider than the within-boot variation. Quote both numbers side by side and the diagnosis is immediate. The architectural consequence is the answer that lands: estimate the bias as a filter state initialised from the stored value rather than trusting the stored value, and add a stationarity detector so the estimator can refine it whenever the robot is still.

What is changing

Learned inertial odometry. Liu et al., "TLIO: Tight Learned Inertial Odometry" (IEEE RA-L, 2020), trains a network to regress a displacement and its uncertainty from a window of raw IMU samples, then feeds that as a measurement into a conventional filter. The framing is what matters: the learned component supplies a statistical motion prior that the physics alone cannot — humans and wheeled robots do not move arbitrarily — while the estimator around it stays classical, auditable and equipped with a fallback. Cioffi et al., "Learned Inertial Odometry for Autonomous Drone Racing" (RA-L 2023), pushed the same idea onto a platform with violent dynamics.

The grade ladder is shifting downward. Industrial MEMS parts with sub-degree-per-hour bias stability now exist at prices that would have bought consumer grade a decade ago, which moves the crossover time in the table above by a factor of several. The design consequence is concrete: the coasting duration you can survive without aiding roughly triples for each factor of 30 in bias stability, since the error is cubic in time.

The invariant to lead with. Whatever changes, the exponents do not. t^1.5 for noise, t² for accelerometer bias, t³ for gyro bias — those come from integration and from gravity, not from manufacturing. Every improvement moves a coefficient. Only aiding — a camera, a LiDAR, a wheel, a satellite — resets the clock, and that is precisely the argument Chapter 6 makes from information theory rather than from anecdote.

Chapter 5: Wheels, Satellites and Radar — Three Sensors, Almost Disjoint Strengths

These three get one chapter because they belong together: each is nearly useless at what the others do well, and that is exactly the property Chapter 6 is going to turn into an argument. The goal here is not to derive all three exhaustively. It is to be able to say, for each one, what it is actually for.

Wheel odometry, and the error that never averages out

Wheel odometry is the highest-rate, lowest-latency, cheapest relative motion source on a ground robot, and it is beautifully precise over one metre and hopeless over one hundred. Understanding why requires separating two kinds of error that behave completely differently.

Systematic errors are wrong constants in the model — a wheel radius that is 1% off, a track width that is 2 mm off. They are deterministic, repeatable, and they grow linearly with distance travelled. Non-systematic errors are slip, a pebble, a bump, a wet patch. They are random and grow as the square root of distance. Borenstein and Feng's UMBmark work established the essential point: on a decent floor, systematic error dominates by an order of magnitude, and it is the one you can actually fix.

Worked example 1 — why 1% is a catastrophe

Differential drive, track width b = 0.50 m, nominal wheel radius 0.10 m. Suppose the right wheel's effective radius is 1% larger than the left's — call that ratio E_d = 1.01. This is a completely ordinary manufacturing and wear tolerance; nobody would notice by looking.

Command the robot to drive straight: both wheels turn through the same angle, so the encoder counts are equal and the odometry confidently reports a straight line. Physically, the right wheel covers 1% more ground, so the robot curves left. Over a commanded arc length s:

• Over s = 1 m: Δθ = 0.01 × 1 / 0.50 = 0.0200 rad = 1.15°.
• Over s = 10 m: Δθ = 0.01 × 10 / 0.50 = 0.200 rad = 11.5°, and the lateral displacement is roughly sΔθ/2 = 1.00 m.

Eleven degrees of heading error over ten metres, from a one-percent wheel mismatch, on a robot whose encoders resolve 0.15 mm. That is the number that ends every "why can't we just use encoders" conversation. And note the structure: the heading error is proportional to distance driven, so it does not average out, it does not shrink with a better encoder, and no amount of filtering removes it — only calibration or an absolute heading reference does.

A pure scale error is milder and equally unfixable by averaging: if both wheels are 1% small in the model, 100 m of driving reports 99 m, a 1 m error. Both errors are calibrated by the same experiment — UMBmark drives a 4 m square in both directions and separates the two by comparing the two return-position clusters, because a scale error is symmetric under reversal and a wheel-ratio error is not.

Worked example 2 — the quantisation floor nobody mentions

An encoder with 1024 cycles per revolution in quadrature gives 4096 counts per revolution. With a 0.10 m wheel the circumference is 2π × 0.10 = 0.6283 m, so one count is 0.6283 / 4096 = 0.1534 mm of travel. Excellent — until you ask for velocity.

At creep speed you are counting one or two ticks per period, so the velocity estimate is essentially a random number. This is why a velocity controller tuned at 1.5 m/s oscillates during a precision docking approach, and why serious drives use timer-capture (measuring the interval between edges rather than the count per period), which inverts the problem: precise at low speed, coarse at high speed. Naming that inversion is a strong answer.

GNSS and RTK, and the failure that reports one centimetre

A GNSS receiver measures the travel time of signals from satellites and solves for position. Two things determine the accuracy, and the second is the one that matters.

Code (pseudorange) solutions correlate the broadcast ranging code. Resolution is limited to a few percent of a chip, and after ionospheric and clock modelling you land at roughly 3 m for standard positioning, or 0.5 m with differential corrections. Carrier-phase solutions measure the phase of the carrier wave itself, whose L1 wavelength is c/1575.42 MHz = 0.1903 m. Phase is measurable to about 1% of a cycle, so the precision is a couple of millimetres — but only modulo one wavelength. You know the fractional part and not the integer number of whole cycles.

RTK resolves those integers using a base station and the geometry over time. When it succeeds you get a fixed solution at 1–2 cm plus about 1 ppm of the baseline (so 1 cm at 10 km). When it does not, you get a float solution at a few decimetres. So far so good.

The observables that catch it, in the order you should name them:

• The ratio test. Ambiguity resolution compares the best integer candidate against the second best. Most receivers expose that ratio; the conventional accept threshold is 3.0. A wrong fix usually shows a ratio just above the threshold, so logging the ratio and alarming on marginal fixes is the cheapest defence there is.
• Discontinuity at re-fix. A correct fix that is re-acquired lands where the previous one was. A wrong fix jumps by ~19 cm or a multiple, instantly, with no dynamics.
• Cross-sensor residual. The fix disagrees with the IMU-propagated position by 38 cm while the IMU has only been coasting for 200 ms — and from Chapter 4 you know that in 200 ms even a consumer gyro contributes under a millimetre. The IMU is right and the satellite is wrong.

Multipath is the other headline failure and it behaves differently. A signal reflecting off a building arrives later than the direct one, so the measured pseudorange is too long, biased in one direction, and correlated over the seconds it takes to drive past the reflector. Two markers distinguish it: the carrier-to-noise ratio C/N₀ dips on the affected satellites, and the error is strongly correlated with satellite elevation — low-elevation satellites are the multipath sources, which is why an elevation mask of 15° is standard. Note the statistical crime: multipath error is time-correlated, so treating consecutive fixes as independent double-counts the information exactly as described in Uncertainty, Least Squares & Robust Costs.

Two more properties worth having ready, because they are asked constantly. A single-antenna receiver cannot observe heading while stationary — heading comes from the velocity vector, which is undefined at rest, so a robot that boots stationary has no yaw until it moves (dual-antenna receivers solve this by measuring the baseline attitude directly). And GNSS delivers absolute, drift-free position at a low rate with high latency — 5 to 10 Hz with 50–150 ms of processing lag — which is the exact photographic negative of an IMU. Coupling architectures are covered in INS/GNSS Coupling.

Worked example — how long is the multipath detour

Put a number on it, because "multipath makes it worse" is not an answer. For a specular reflection off a vertical plane at perpendicular distance d from the antenna, with the satellite at elevation E and its azimuth offset from the wall normal by Δaz, the mirror-image construction gives the extra path length as

• Robot 8 m from a warehouse wall, satellite at 20° elevation, 60° off the wall normal: Δρ = 2 × 8 × 0.9397 × 0.5 = 7.52 m.
• Robot 20 m from a building, satellite at 30° elevation, 45° off normal: Δρ = 2 × 20 × 0.8660 × 0.7071 = 24.5 m.

A C/A code chip is c/1.023 MHz = 293 m long, and a receiver's correlator only rejects reflections delayed by more than about 1.5 chips (440 m). Both of these are far inside the vulnerable window, so the reflected energy blends into the correlation peak and pulls it late. The induced pseudorange error is a fraction of Δρ set by the reflection's relative amplitude — typically up to half — so a few metres. Two structural facts follow: the error is positive-biased (a detour is never shorter), and it is correlated over the seconds you spend near that wall, which is exactly the condition under which treating consecutive fixes as independent will over-confidently drag your filter.

The heading problem, and why nobody uses a magnetometer

Heading is the awkward state. The gyro gives you heading rate and drifts; single-antenna GNSS gives you course over ground only while you are moving; a magnetometer promises absolute heading for two dollars. Then you put it on a robot.

A current-carrying wire produces a field B = μ₀I/(2πr), and μ₀/(2π) = 2 × 10^-7 T·m/A. Earth's horizontal field is about 20 µT at mid-latitudes. So:

Those errors are load-dependent, so they change with acceleration, with payload, and with the slope of the floor — a hard-iron calibration performed at rest corrects none of it. Add a steel shelf rack, a forklift driving past, and the reinforcing bar in the concrete, and you have an absolute heading reference that is worse than the drift it was meant to correct. The answer that lands: use a dual-antenna GNSS receiver if you need absolute heading outdoors, use GNSS course-over-ground plus gyro once moving, and treat a magnetometer as a coarse initialisation with a ±30° gate — never as a measurement your filter trusts.

Radar, and the resolution nobody expects

Automotive FMCW radar transmits a linear frequency chirp of bandwidth B over duration T_c, mixes the echo with the transmitted waveform, and reads the beat frequency, which is proportional to round-trip delay:

With B = 1 GHz, T_c = 50 µs and a target at R = 30 m: f_b = 2 × 1e9 × 30 / (2.998e8 × 50e−6) = 6e10 / 14990 = 4.00 MHz — comfortably digitised by a cheap ADC, which is a large part of why radar is cheap.

Three resolutions follow, and their ratio is the whole story.

1. Range resolution ΔR = c/(2B). At B = 1 GHz that is 2.998e8/2e9 = 0.150 m; at 4 GHz (the 77–81 GHz band) it is 0.0375 m. Excellent.
2. Velocity comes from the phase advance of the target between successive chirps — the Doppler shift. At 77 GHz the wavelength is λ = 2.998e8/7.7e10 = 3.893 mm, and the unambiguous velocity span is ±λ/(4T_c) = ±0.003893/(4 × 5e−5) = ±19.5 m/s. Velocity resolution is typically a few cm/s. Outstanding — and measured directly, in one frame, with no association and no tracking.
3. Angular resolution is set by the aperture: Δθ ≈ 2/N radians for N virtual antenna elements spaced λ/2. With N = 8: 2/8 = 0.25 rad = 14.3°. At 50 m that is a cross-range blur of 50 × 0.25 = 12.5 m. Catastrophic.

What it buys you in exchange is the property no optical sensor has: 77 GHz passes through rain, fog, dust, snow and darkness essentially unattenuated, because the wavelength is thousands of times longer than the droplets that scatter light. Chapter 6 will make this precise, but hold the shape of it: radar's failure modes are almost perfectly uncorrelated with camera's and LiDAR's, which is worth more than any single specification.

Radar's own failures are specific and you should name them: ghost targets from multipath under and between vehicles (a bounce off the road and then a car produces a detection at a range that is the sum of two legs); ground clutter, which is why raw radar needs constant-false-alarm-rate detection rather than a fixed threshold; near-zero elevation resolution on most units, so an overhead sign and a stopped vehicle are the same detection — the failure behind several well-known highway-automation incidents; and micro-Doppler smearing on rotating or articulated targets.

Worked example 3 — what 192 virtual channels buys

Imaging radar increases N by using MIMO: N_tx transmitters and N_rx receivers synthesise N_tx × N_rx virtual elements. With 12 × 16 = 192:

That table is the entire "4D imaging radar" story in one place, and it explains why the category appeared: angular resolution improves as 1/N, so getting from useless to useful took a 24× increase in channel count, which took a decade of silicon.

What to look for. Set E_d = 1.010 and the odometry loop opens by metres — then set it back to 1.000 and add twelve slip events, and notice that the error is far smaller and differently shaped: slip produces a jagged random walk, a wheel-ratio error produces a smooth systematic curl. That difference is diagnostic. Then switch the satellites to "wrong integer fix": the dots stay tight and consistent, they are simply offset by 38 cm, and nothing about their scatter reveals it.

Three consumers, three latencies

The design question here is never "how do I read an encoder". It is: given three sources with wildly different rates, latencies and error structures, what does each one get to influence?

The latency budget that decides the architecture. A GNSS fix stamped for time t arrives at t + 120 ms. At 3 m/s the robot has moved 36 cm since that measurement was valid. You therefore cannot apply it as a correction to the current state; you must either keep a short state buffer and re-apply the intervening motion, or run an out-of-sequence update. Getting this wrong produces a position estimate that oscillates at the GNSS rate with amplitude equal to latency times speed — a distinctive 5 Hz sawtooth of 36 cm. Recognising that sawtooth on a plot is a party trick worth having.

The ordering constraint. Every one of these three needs a gate before it reaches the estimator, and the gate needs different evidence for each: a slip detector for odometry (compare wheel-derived yaw rate against the gyro — they disagree only when a wheel is slipping), the ambiguity ratio and a chi-square innovation test for GNSS, and a CFAR plus a track-consistency check for radar. A design that draws three arrows into a filter with no gates is the single most common weak answer to this prompt.

The bandwidth footnote. All three together are 108 kB/s — a thousandth of the camera. Their cost is never bytes; it is the timing discipline and the gating logic. Say that explicitly, because it reframes the whole design conversation away from throughput and toward correctness.

Dead reckoning with the error model in it

python
import numpy as np

def dead_reckon(dl, dr, b, Ed=1.0, Es=1.0):
    # dl, dr: per-step wheel arc lengths from encoder counts (metres).
    # Ed: right/left effective radius ratio. Es: overall scale error.
    # The TRUE motion uses the real radii; odometry uses the nominal ones,
    # which is why the two diverge even with perfect encoders.
    x = y = th = 0.0
    P = np.zeros((3, 3))
    kl, kr = 1e-4, 1e-4            # slip variance per metre travelled
    path = []
    for i in range(len(dl)):
        L, R = Es * dl[i], Es * Ed * dr[i]
        ds, dth = (L + R) / 2.0, (R - L) / b
        # midpoint integration — first order in dth, and enough at 50 Hz
        x  += ds * np.cos(th + dth / 2)
        y  += ds * np.sin(th + dth / 2)
        th += dth
        # covariance: Jacobians wrt state and wrt the two wheel arcs
        Fx = np.array([[1, 0, -ds * np.sin(th)],
                       [0, 1,  ds * np.cos(th)],
                       [0, 0,  1]])
        Fu = np.array([[0.5 * np.cos(th), 0.5 * np.cos(th)],
                       [0.5 * np.sin(th), 0.5 * np.sin(th)],
                       [-1.0 / b,        1.0 / b]])
        Q = np.diag([kl * abs(L), kr * abs(R)])
        P = Fx @ P @ Fx.T + Fu @ Q @ Fu.T
        path.append((x, y, th, np.sqrt(P[0, 0] + P[1, 1])))
    return path

Two things to point out while you write it. The noise Q is proportional to distance travelled, not to time — a stationary robot accumulates no odometry uncertainty, and getting that wrong is the classic reason a parked robot's covariance balloons overnight. And the covariance this produces models only slip; it says nothing about E_d or E_s, which are biases. If those are unknown, they belong in the state vector or in a calibration, never in Q, because inflating Q to cover a bias makes the filter humble in the wrong direction — it widens the ellipse symmetrically while the true error walks off in one direction.

The GNSS side, where the interesting code is entirely in the gating:

python
def accept_fix(fix, pred, P_pred):
    # fix.status: 0=none 1=single 2=float 4=fixed  |  fix.ratio: AR test statistic
    if fix.status != 4:
        R = np.diag([0.5, 0.5, 1.0]) ** 2       # float: decimetres, not centimetres
    elif fix.ratio < 3.0:
        return None                            # marginal integer fix — trust nothing
    else:
        R = np.diag([0.015, 0.015, 0.03]) ** 2
    y = fix.pos - pred                          # innovation
    S = P_pred + R
    d2 = y @ np.linalg.solve(S, y)
    if d2 > 16.27:                            # chi-square, 3 DOF, 99.9%
        log_warn("fix rejected: d2=%.1f, err/lambda=%.2f" % (d2, np.linalg.norm(y) / 0.1903))
        return None
    return R

And the radar side, which is the one people least expect to be simple. The entire signal chain is two Fourier transforms:

python
import numpy as np
c, B, Tc, lam, M = 3e8, 1e9, 50e-6, 3.893e-3, 128

def range_doppler(iq):
    # iq is (n_fast, n_chirps) complex baseband. FAST time -> range,
    # SLOW time (chirp index) -> velocity. That is the whole algorithm.
    win_f = np.hanning(iq.shape[0])[:, None]
    win_s = np.hanning(iq.shape[1])[None, :]
    Rmap = np.fft.rfft(iq * win_f, axis=0)          # beat frequency -> range bin
    RD = np.fft.fftshift(np.fft.fft(Rmap * win_s, axis=1), axes=1)
    rng = np.arange(RD.shape[0]) * c / (2 * B)   # 15 cm per bin at B = 1 GHz
    vel = np.arange(-M//2, M//2) * lam / (2 * M * Tc)  # 0.30 m/s per bin
    return np.abs(RD), rng, vel

def cfar_1d(x, n_train=16, n_guard=4, alpha=8.0):
    # constant false alarm rate: the threshold is LOCAL, because clutter
    # power varies enormously with range. A fixed threshold is unusable.
    k = n_train + n_guard
    pad = np.pad(x, k, mode='edge')
    win = np.ones(2*k + 1); win[k-n_guard:k+n_guard+1] = 0
    noise = np.convolve(pad, win / win.sum(), mode='same')[k:-k]
    return x > alpha * noise

Three numbers to quote while writing it. The range axis is c/(2B) per bin, so 15 cm at 1 GHz of chirp bandwidth. The velocity axis is λ/(2 M T_c) per bin, so with 128 chirps of 50 µs that is 0.30 m/s — and the whole frame took 128 × 50 µs = 6.4 ms, which is why radar latency is trivial compared to a LiDAR sweep. And the angle is not in this code at all, because it comes from combining the range-Doppler maps of the N virtual receive channels, which is exactly why angular resolution is a function of channel count rather than of signal processing.

The cfar_1d function is the part that separates people who have used radar from people who have read about it: the detection threshold must be local, because ground clutter power spans orders of magnitude across the range axis, and a fixed threshold produces either a wall of false alarms up close or complete blindness far away.

The log line is the part worth stealing. Printing the rejected innovation in units of L1 wavelengths means that when someone pastes the log into a chat, a value of 1.99 or 3.01 tells you instantly that the receiver resolved the integers wrongly rather than that the robot jumped. Instrumenting for the failure mode you predicted is the difference between a system you can debug remotely and one that requires a site visit.

Two failures, and the metric that separates them

Failure 1: RTK says 1.4 cm, ground truth says 38 cm

Symptom. A yard robot's fused position steps sideways by about 38 cm, holds there for ninety seconds, then steps back. During the whole episode the receiver reports status fixed and a horizontal sigma of 0.014 m. The IMU and wheels agree with each other and disagree with the satellite.

Root cause. A wrong integer ambiguity resolution: 38 cm is 1.997 L1 wavelengths.

The metric that reveals it, and the three decoys it beats. Divide the error by 0.1903 m and look at how close the result is to an integer. Then check three more things, each of which eliminates a competitor:

Each decoy is eliminated by a different observable — step versus ramp, body-frame versus world-frame, session-constant versus transient. Walking through that table beats naming the right cause immediately, because it shows the reasoning transfers.

Failure 2: odometry drifts, and the fix makes it worse

Symptom. A robot closes a 60 m loop about 2.5 m short and rotated. Someone inflates the odometry covariance so the filter trusts it less. Loop closure improves slightly; the estimate now jitters visibly between LiDAR updates and the controller starts hunting.

Root cause. The error is systematic (a wheel-ratio bias), and covariance describes random error. Inflating Q tells the filter "this measurement is noisy", which is false — it is precise and wrong. The filter responds by weighting odometry less everywhere, including in the short-horizon prediction where it was the best thing available, so the prediction degrades and the corrections get larger and more visible.

The metric that reveals it. Plot the odometry-versus-truth error against distance driven on log axes. A random error gives slope 0.5; a systematic one gives slope 1.0. Here the slope is 1.0, which says: calibrate, or estimate the parameter, but do not model it as noise. The confirming experiment is UMBmark — drive the same square clockwise and anticlockwise. Random error gives two overlapping clusters of return points; a wheel-ratio bias gives two clusters displaced in opposite directions, and the separation between their centroids gives you E_d directly.

What is changing

Radar became a geometry sensor, barely. High-channel-count MIMO units (Continental ARS540, Arbe, Uhnder's digital-code modulation) took N from 8 to 192 or more, dropping angular resolution from 14° to under 1° and adding usable elevation. That crosses the threshold where radar detections can be clustered into objects with extent rather than treated as bare range-rate reports. The invariant to hold on to: Δθ ≈ 2/N, so every claim about imaging radar is really a claim about channel count.

Radar odometry stopped being a curiosity. Barnes, Gadd, Murcutt, Newman and Posner, "The Oxford Radar RobotCar Dataset" (ICRA 2020), and Barnes and Posner, "Under the Radar: Learning to Predict Robust Keypoints for Odometry Estimation and Metric Localisation in Radar" (ICRA 2020), showed that a scanning radar can carry a localisation stack on its own through weather that removes both camera and LiDAR. That is the sharpest possible demonstration of the complementarity argument in the next chapter.

GNSS got more constellations and better correction models. Multi-constellation, multi-frequency receivers (GPS, Galileo, BeiDou, GLONASS on L1/L2/L5) shorten time-to-fix and make wrong fixes rarer, and network RTK plus PPP-RTK removes the single-base-station dependency. None of it removes the failure mode: a wrong fix is still reported as fixed. The gate is the engineering, not the receiver.

Wheel odometry did not change at all, and that is worth saying. It is the same differential-drive kinematics it was in 1990, with the same E_d and E_s, calibrated by the same square-path experiment. Asked what is new here, the honest answer is "nothing, and that is a feature — it is the one measurement on the robot whose model I trust completely, right up to the instant a wheel slips, which is why the interesting work is slip detection, not slip modelling."

Chapter 6: Why Fusion Works — The Argument From Physics, Not From Enthusiasm

"We fuse the sensors" is what everybody says. It is not an argument — it is a slogan. There is a real argument, it takes four steps, and every step is arithmetic.

Step 1: information adds, and that alone buys you almost nothing

Two independent measurements of the same scalar, with variances σ₁² and σ₂². The maximum-likelihood combination weights each by its precision (the reciprocal of its variance), and the result has

Precisions add. That is the whole of linear fusion, and it is why the information form of the Kalman filter is the natural one for multi-sensor work — adding a sensor is literally adding a matrix. Now take it seriously with numbers.

Fuse a good LiDAR-based position estimate (σ₁ = 0.10 m) with a worse one from a second LiDAR of the same type (σ₂ = 0.30 m):

1. 1/σ₁² = 1/0.01 = 100
2. 1/σ₂² = 1/0.09 = 11.111
3. Sum = 111.111, so σ_f² = 1/111.111 = 0.009000
4. σ_f = 0.09487 m

Step 2: fusion pays when the sensors are good at different things

Now the same arithmetic in two dimensions, where covariance is a shape rather than a number. Sensor A knows the x direction well and y poorly — think of a LiDAR looking down a long corridor, which pins your lateral position against the walls and tells you very little about where you are along it. Sensor B is the reverse: a wheel odometer, precise along the direction of travel, mute about lateral offset.

Invert each to get its information matrix, add them, invert back:

1. Σ_A⁻¹ = diag(400, 1).   Σ_B⁻¹ = diag(1, 400).
2. Sum: diag(401, 401).
3. Invert: Σ_f = diag(0.0024938, 0.0024938), so σ_x = σ_y = 0.04994 m.

Compare the areas of the one-sigma ellipses, which is the honest measure of how much you know about a 2-D quantity. Area = π√detΣ.

A 20-fold reduction in the area of the uncertainty region — against a 5% improvement in the same-axis case. Same equation, same amount of hardware, four hundred times the payoff. The difference is entirely in the orientation of the two information ellipsoids.

Step 3: correlation quietly destroys the gain

The addition rule assumed independence. Real sensors share error sources: two cameras behind the same windscreen see the same rain; two LiDARs on the same mast share its flex; every sensor on the robot shares the same clock, the same calibration campaign, the same extrinsic reference frame. Redo the derivation with correlation ρ.

For two scalar estimates with correlation ρ, the optimal weight and the resulting variance are

Take two identical sensors, σ₁ = σ₂ = 0.10 m. The naive independent answer is 0.10/√2 = 0.07071 m. Now sweep ρ:

Work the ρ = 0.90 row so you can reproduce it. Numerator: 0.01 × 0.01 × (1 − 0.81) = 1e−4 × 0.19 = 1.9 × 10^-5. Denominator: 0.01 + 0.01 − 2 × 0.9 × 0.1 × 0.1 = 0.02 − 0.018 = 0.002. Ratio: 1.9e−5/0.002 = 0.0095, so σ_f = 0.09747 m. The second sensor bought a 2.5% improvement, not 29%.

Notice the limit: as ρ → 1, σ_f → σ₁. A perfectly correlated second sensor adds exactly zero information, which is obvious once stated and is exactly what a naive fusion refuses to believe. And the failure is silent — the filter reports a shrinking covariance while the error does not shrink. That is the "filter that lies" from Uncertainty, Least Squares & Robust Costs, arriving through a different door.

Two defences, and you should know both. If you can estimate the correlation, use the formula above. If you cannot — which is the usual case, because the shared error sources are exactly the ones you did not model — use covariance intersection:

The weights sum to one instead of both being one, which makes the result consistent for any correlation whatsoever. The price: on our orthogonal 2-D example the optimal-w covariance-intersection result is σ = 0.0706 in both axes against the Kalman answer's 0.0499 — twice the ellipse area. And for two identical sensors, covariance intersection returns σ = 0.10, correctly refusing to claim any improvement at all. Conservative and never wrong, versus optimal and wrong whenever you were mistaken about independence. Being able to state that trade is the answer to "how do you fuse two estimates that might share information".

Step 4: fusion makes unobservable things observable

Error reduction is the boring half. The important half is that some quantities are not observable at all to a given sensor, at any noise level, for any amount of time — and adding a second modality can make them observable. This is a different kind of argument and it is the one that actually justifies most real sensor suites.

Now pair them and watch the holes fill. Monocular camera plus IMU is the canonical case, and the mechanism is worth deriving because it is the reason VIO exists at all.

Worked example — how an IMU gives a camera its scale, and when it refuses to

Monocular visual odometry produces a trajectory known only up to an unknown scale s: the camera reports a displacement of, say, 0.05 "visual units" over half a second, and 0.05 units could be five centimetres or five metres.

The accelerometer measures metric specific force. Double-integrate it over the same window and you have a metric displacement. Setting the two equal gives one equation in the one unknown:

With a constant acceleration a = 1.0 m/s² over T = 0.5 s, the inertial displacement is ½ × 1.0 × 0.25 = 0.125 m. So s = 0.125 / 0.05 = 2.5 metres per visual unit. Scale recovered.

Now the crucial follow-up, which is where the real engineering lives: how well is it recovered, and when does this fail? The IMU's own displacement noise over 0.5 s with a VRW of 0.1 m/s/√hr is N·T^1.5/√3 = 0.001667 × 0.3536 / 1.732 = 0.340 mm. Divide that by the inertial displacement and you get the relative uncertainty on the scale:

The suite is chosen so hazards do not coincide

Everything above assumed the sensors are healthy. The last step of the argument is about what happens when they are not, and it is a statement about correlation between failure events rather than between measurement errors.

Two rows carry the entire design lesson. Rain and fog degrade camera and LiDAR together — they are both optical, so their failures are correlated, and a camera-plus-LiDAR suite has a single point of environmental failure no matter how carefully you fuse them. That is the whole argument for putting a 77 GHz radar on an outdoor robot: not that radar is good, it is mediocre at almost everything, but that its failures are uncorrelated with the optical ones.

And fast rotation degrades camera and LiDAR together too, for the reasons in Chapters 1 and 3 — both integrate over time, so both smear. The sensor that gets better during fast rotation is the gyro, whose signal-to-noise ratio rises with the signal. That complementarity is not a coincidence; it is why the IMU sits at the centre of every modern estimator.

What to look for. At 0° the two ellipses lie on top of each other and the fused one barely shrinks — that is the 5% case. Sweep to 90° and it collapses to a small circle — the 20× case. Now add correlation: the solid fused ellipse (what the filter reports) does not move, while the dashed honest one grows. The gap between them is the lie, and it is invisible from inside the filter.

Worked example — how often do you need an absolute fix?

This is the calculation that turns the whole lesson into a specification, and it is the single most useful thing in this chapter. It combines Chapter 4's drift law with a fusion budget, and it takes twenty seconds.

Between absolute fixes the robot coasts on the IMU, so its error grows as g·b_gt³/6. Give the drift a budget — say it may contribute at most 5 cm before the next fix arrives — and invert:

With a consumer gyro at 30°/hr = 1.4544 × 10^-4 rad/s, the coefficient g·b_g/6 = 9.81 × 1.4544e−4 / 6 = 2.378 × 10^-4 m/s³. So T_max = (0.05 / 2.378e−4)^1/3 = (210.3)^1/3 = 5.95 s.

Note the cube root: a 100× improvement in gyro bias buys only a 4.6× longer coast. That is a brutal exchange rate, and it is why almost every real system chooses to buy aiding availability rather than gyro grade. Ten thousand dollars of tactical gyro extends your tunnel from six seconds to twenty-eight; a second modality that works in the tunnel removes the limit entirely.

The steady-state error follows from combining the fix accuracy with the coast: with a 3 cm LiDAR fix and a consumer gyro, at a 1 s aiding interval the drift contributes 0.2 mm and the answer is just the fix accuracy, 3.0 cm; at 5 s the drift is 3.0 cm and the combined error is 4.2 cm; at 10 s the drift is 23.8 cm and it swamps everything. The aiding interval at which drift equals fix accuracy is the design point, and computing it out loud is one of the highest-value thirty seconds you can spend in a system-design round.

Where the fusion happens, and what it costs

The design question is not "do we fuse" but "at what level", and there are three answers with three different cost structures.

The number that decides it. A loosely coupled GNSS fusion needs four satellites to produce a pose; below that it contributes nothing. A tightly coupled one uses every pseudorange it gets, so two satellites in an urban canyon still constrain two directions. On a site with intermittent sky view that is the difference between a usable and an unusable system, and it costs about 4 kB/s of extra plumbing. That trade — tight coupling degrades gracefully, loose coupling degrades cliff-edge — is the design sentence to have ready.

The layer everyone forgets: fault detection and exclusion. Fusion without gating is worse than no fusion, because it launders a bad measurement into a confident state. The architecture that actually ships has three parts:

The degradation ladder is the part that impresses, because it forces you to say what the robot does rather than what it computes. State it in terms of observability: "with LiDAR and camera both down in fog, I still observe along-track motion from the wheels and heading rate from the gyro, but nothing pins absolute position — so the robot may continue at reduced speed along a known route for a bounded distance derived from the drift law, and must stop before the accumulated uncertainty exceeds the aisle clearance." Chapter 4's cubic gives you that bound in metres.

Information-form fusion, and the conservative alternative

python
import numpy as np

def fuse_information(means, covs, valid):
    # The whole of multi-sensor fusion, when the sensors are independent:
    # add the information matrices, add the information vectors, invert once.
    n = len(means[0])
    Y = np.zeros((n, n))          # information matrix  = sum of Sigma_i^-1
    y = np.zeros(n)               # information vector  = sum of Sigma_i^-1 mu_i
    for mu, S, ok in zip(means, covs, valid):
        if not ok:                # a degraded sensor contributes ZERO, not a guess
            continue
        Oi = np.linalg.inv(S)
        Y += Oi
        y += Oi @ mu
    P = np.linalg.inv(Y)
    return P @ y, P

def fuse_ci(S1, S2, w=0.5):
    # covariance intersection: consistent for ANY unknown correlation,
    # because the weights sum to one instead of both being one.
    return np.linalg.inv(w * np.linalg.inv(S1) + (1 - w) * np.linalg.inv(S2))

Three things to say while writing it. "The valid mask is the whole safety story — a degraded sensor must contribute zero information, not inflated information, because inflating a covariance still lets a wrong mean pull the estimate." "I invert once at the end, not per sensor, and in production I would keep the state in information form so there is no inversion in the loop at all." And "if I cannot argue that these sensors are independent, this function is optimistic and I should be using the second one."

Sanity-check the code against the hand arithmetic: with the two orthogonal sensors above, fuse_information returns diag(0.0024938, 0.0024938) and fuse_ci at w = 0.5 returns diag(0.005, 0.005) — exactly twice the area, which is the price of not knowing the correlation.

Two failures, and the metric that separates them

Failure 1: the second camera shrank the covariance and not the error

Symptom. A team adds a second front camera. The reported position sigma drops from 8.0 cm to 5.7 cm exactly as the √2 rule predicts. Measured against motion capture, the RMS error does not move at all — it stays at 8.1 cm.

Root cause. The two cameras sit 12 cm apart behind the same window, on the same bracket, calibrated in the same session against the same target, sharing the same clock. Their errors are correlated at something like ρ = 0.9, so the true fused sigma is 0.0975 — barely better than one camera — while the filter, assuming independence, claims 0.0707.

The metric that reveals it. Log both cameras' innovation sequences against the filter and compute their cross-correlation at lag zero. Independent sensors give a value near zero with a noise floor of 1/√N. Here it will sit near 0.9, and the prediction is quantitative: with ρ = 0.9 the expected NIS is 1.90 rather than 1.00, so a NIS monitor sitting on the fused update reads about twice its nominal value. Two numbers — the cross-correlation and the NIS — and both are computable without ground truth, which means they run on a customer's robot.

The decoy. The obvious wrong diagnosis is "the per-camera covariance is too optimistic; inflate R". That would also raise NIS. It is ruled out by the cross-correlation: an over-tight R inflates each camera's innovations independently and leaves them uncorrelated with each other. Correlation between the two streams is specific to a shared error source, and the fix is different too — move a camera, re-calibrate them separately, or fuse with covariance intersection.

Failure 2: the fused estimate is worse than the best single sensor

Symptom. LiDAR-only localisation runs at 4 cm RMS. Add GNSS and the fused estimate is 40 cm RMS with occasional 2 m excursions. Somebody proposes weighting GNSS down.

Root cause. An ungated sensor with a heavy-tailed error distribution. In the yard, a few per cent of the GNSS fixes are multipath-biased or wrongly fixed, and because they are handed to the filter with a 1.5 cm covariance, each one is a 100-sigma pull on a state that was previously correct.

The metric that reveals it. Per-sensor NIS, plotted as a histogram rather than a mean. A sensor that is uniformly too confident shows a whole distribution shifted upward — median and p95 both high. A sensor with occasional gross outliers shows a normal median and an enormous p99. Here the GNSS NIS median will be near 1.0 and its p99 in the thousands. That distinction is the diagnosis: median tells you about the noise model, the tail tells you about outliers, and they need different fixes.

Weighting the sensor down globally is the wrong fix and it is worth being able to say why: it discards the 97% of fixes that were correct in order to survive the 3% that were not, so you lose the absolute position information you added GNSS for in the first place. The right fix is a chi-square gate plus the ambiguity-ratio check from Chapter 5 — reject the bad fixes and keep the good ones at their true, tight covariance.

What is changing

Learned fusion moved from late to intermediate. Liu et al., "BEVFusion: Multi-Task Multi-Sensor Fusion with a Unified Bird's-Eye View Representation" (ICRA 2023), projects camera and LiDAR features into a shared bird's-eye-view space and fuses there, rather than fusing detections after the fact. The property that matters for this chapter: because the fusion happens in a shared representation, the network can use camera evidence where LiDAR is sparse and vice versa — which is the orientation argument from step 2, learned rather than hand-specified. The property that should worry you: nothing in the architecture guarantees graceful behaviour when one modality is degraded rather than absent, and a confidently wrong LiDAR (a mirror) is exactly that case.

Degradation-aware fusion is the open problem. The honest state of the art is that most learned fusion assumes both modalities are healthy, and the robustness work — modality dropout during training, per-modality confidence heads — is active rather than settled. This is a good thing to say when asked about the frontier, because it is true and it shows you can distinguish a benchmark result from a shipped system.

The camera-only argument, and how to handle it. The question of whether a vision-only stack suffices comes up on every team, and the strongest position is neither yes nor no: "Camera-only is defensible when your failure taxonomy shows no hazard that removes a spanning set of the states you need, and when you have the data scale to cover the tail. On a highway with a large fleet that argument can be made. In a warehouse with shrink-wrapped pallets, glass partitions and forklifts, the mirror and transparency failures are confident-wrong rather than absent, and no amount of camera data fixes a physically ambiguous measurement — so I would keep a second modality whose physics differs. What would change my mind is a measured failure rate on the specific hazards, not a benchmark." That answer names the criterion instead of picking a tribe, and criteria are what good decisions are made of.

Chapter 7: Showcase — The Sensor-Suite Failure Bench

Everything in the previous six chapters was a claim about one sensor at a time. This chapter puts all seven on one robot, drives it round a loop, and lets you break them one at a time to watch what the suite does about it.

The bench is not decorative. Every degradation factor below comes from a number derived earlier in the lesson, and the fused uncertainty is computed by literally adding information matrices — the arithmetic of Chapter 6, running live. If the picture surprises you, the surprise is in the physics, not in the animation.

How to read it

The top panel is the warehouse loop from above. The robot drives it continuously. When you enable a hazard, a coloured arc appears on the section of the loop where that hazard lives, and the robot's sensors degrade while it is inside that arc and recover when it leaves.

The bottom panel is the live sensor dashboard. Each bar is one sensor's precision — the reciprocal of its variance — on a logarithmic scale, split into the two directions that matter for a robot in an aisle: along-track (how far down the aisle you are) and cross-track (how far from the shelf you are). The bottom bar is the fused result, computed by summing every healthy sensor's precision in each direction.

The two numbers to watch are on the last row: the fused sigma, and the sigma of the best single sensor at that instant. The whole point of the bench is what happens to the gap between them.

The model behind the bars

Each sensor contributes a precision in each direction. The nominal values are the ones this lesson computed:

Look at the wheel and radar rows. Both are terrible in one direction and respectable in the other — exactly the anisotropy that Chapter 6 showed is worth twenty times more than raw accuracy. And look at the IMU: 60 cm, the worst absolute number in the table, and it is the sensor that never degrades in any row of the hazard matrix.

The hazards multiply those precisions by the factors below. Each one traces to a mechanism from an earlier chapter.

Six experiments to run, and what each one proves

1. Enable only "tunnel". The GNSS bar goes to zero and the fused number barely moves. This is Chapter 6 step 1 in action: GNSS at 1.5 m was contributing 1/2.25 = 0.44 units of precision against a total in the hundreds. Losing your worst sensor is not a story. Tunnels sound scary; the real answer is that indoors GNSS never mattered.
2. Enable only "glass bay". The LiDAR — the best sensor by a factor of seven — collapses, and the fused sigma degrades from about 2.7 cm to around 7 cm. Still usable, because the camera and stereo were carrying real information all along. This is what a well-chosen suite looks like: losing the best sensor is a degradation, not a failure.
3. Enable "darkness" alone, then "rain" alone, then both. Darkness removes the two passive optical sensors outright. Rain weakens all three optical ones. Together they take the fused sigma from about 2.7 cm to 5.3 cm — a doubling from two hazards that a naive suite audit would have counted as "we still have five working sensors". This is correlated failure, and the reason the number is not far worse is the radar and the wheels, whose physics is untouched by either. That is the argument for putting a mediocre 77 GHz sensor on an outdoor robot, made visible.
4. Enable "featureless corridor" and watch the two directions separately. Cross-track precision barely moves — the walls are still there and the LiDAR still sees them, so the lower bar stays near 3 cm. The upper, along-track bar falls off a cliff. This asymmetry is the single most important thing on the bench, and it is invisible if you only track a scalar "confidence". A suite can be healthy in one direction and nearly blind in another at the same instant.
5. Enable "featureless corridor" and "wheel slip" together — their zones overlap, which is realistic, because polished featureless aisles are exactly where wheels slip. Along-track sigma goes to about 0.16 m while cross-track stays near 0.030 m, a five-to-one anisotropy. Add darkness on top and along-track reaches 0.18 m. That is the state in which a robot should slow down — and it is a state a single scalar health metric would report as "mostly fine", because the average across directions still looks respectable.
6. Press "all hazards on" and let it drive. Watch the fused number step as the robot enters and leaves each zone rather than sitting at one degraded value — because hazards are localised, and that is the honest picture. Find the worst point on the loop (it is where darkness overlaps the featureless corridor and the slip zone) and read the along-track sigma there. Knowing exactly what that worst number is, and where on the route it occurs, is what lets you write a degradation policy instead of guessing at one.

What the bench deliberately does not model

Being explicit about a model's limits is itself an engineering skill, so here are the three simplifications, each of which makes the real world worse than the picture:

• Independence. Every bar is summed as though the sensors' errors were uncorrelated. In reality they share a clock, a calibration campaign, a chassis and a set of extrinsics, so the true fused sigma is larger than shown — by up to 40% at ρ = 0.9, per the table in Chapter 6.
• Confident-wrong is not represented. Every hazard here reduces precision, which is the polite failure. A mirror return, a wrongly-fixed RTK ambiguity or a repetitive-texture stereo match would keep the bar at full height and move the mean. That class is invisible to any confidence bar and can only be caught by cross-sensor disagreement — which is why the fault-detection layer in Chapter 6 sits outside the fusion, not inside it.
• The IMU bar is a snapshot, not a constant. Its 0.60 m assumes a few seconds since the last absolute fix. Under sustained hazards its precision falls as t^-3, so the "all hazards on" residual is not a steady state — it is a clock running down at the rate Chapter 4 derived. On this robot, at consumer gyro grade with a 20 cm budget, that clock is about 9.4 seconds long.

That third point is the one to volunteer unprompted. "The fallback is not a floor, it is a countdown whose length I can compute" — one sentence that carries both the drift law and the safety argument.

Chapter 8: Field Guide

Sooner or later somebody walks up to your desk and says: "Our AMR localises fine in aisle 3 and loses itself in aisle 7. Aisle 7 has a glass partition. Ten minutes — what do you check and in what order?" Everything in this chapter exists to make the next ninety seconds automatic. Nothing here is new material; it is the six chapters above compressed into the form you need when the clock is real — the three derivations you should be able to do on a blank page, the numbers you can produce from memory, the failure table you walk when the symptom arrives, and the drill that trains the retrieval speed.

0 · The three derivations you should be able to do from memory

The test that separates recall from understanding is getting a result rather than stating it. In sensing there are three such results, and they come up so reliably that they are worth over-learning. Each one is five steps. There are no gaps in what follows — if a step is not obvious, that is the step to rehearse.

Derivation 1 — why stereo depth error is quadratic in range. Draw two cameras and ask: how does depth uncertainty grow?

1. Start from the measurement equation. Two rectified cameras separated by baseline B, focal length f in pixels. A world point at depth Z projects into the two images at a horizontal offset d = u_L − u_R pixels, and similar triangles give Z = fB/d. Say out loud what is measured: d. Z is inferred.
2. Differentiate the inference with respect to the measurement. Z = fB·d⁻¹, so dZ/dd = −fB·d⁻² = −fB/d². The minus sign is physical — more disparity means nearer — and we drop it when we propagate magnitudes.
3. Propagate the uncertainty through the first-order term. For a scalar function, σ_Z = |dZ/dd| σ_d, so σ_Z = fBσ_d/d². This is already correct, but it is expressed in the wrong variable: nobody asks "what is my error at 30 pixels of disparity", they ask "what is my error at 15 metres".
4. Substitute the measurement back out. From step 1, d = fB/Z, hence d² = f²B²/Z². Put that in the denominator: σ_Z = fBσ_d · Z²/(f²B²).
5. Cancel one factor of fB. σ_Z = Z²σ_d/(fB). Now read it aloud: error grows as the square of range, and fB is the single number that buys it down. Doubling the baseline halves the error at every range and costs you the near field, because d = fB/Z exceeds the search range when Z is small.

Once the derivation is down, the follow-up is always numeric: f = 700 px, B = 0.12 m, σ_d = 0.2 px gives fB/σ_d = 84/0.2 = 420, so σ_Z = Z²/420 — 2.4 cm at 3 m, 54 cm at 15 m.

Derivation 2 — why gyro bias produces a cubic position error. The question: your IMU has 1°/hr of bias — how far do you drift in twenty seconds? The trap is to integrate the gyro twice, which gives an angle error, not a position error. The position error comes from gravity.

1. The bias becomes a tilt. A constant gyro bias b_g (rad/s) integrates into an attitude error that grows linearly: θ(t) = b_gt. After 20 s at 1°/hr = 4.848×10^-6 rad/s, that is 9.70×10^-5 rad — five thousandths of a degree. Nothing, so far.
2. The tilt mis-resolves gravity. The accelerometer measures specific force, and the navigation equations subtract the estimated gravity vector. If your estimate of "down" is tilted by θ, the subtraction leaves a residual in the horizontal plane of magnitude a_horiz = g sinθ. This is the step everyone skips, and it is the whole answer.
3. Linearise, because θ is tiny. sinθ ≈ θ to better than one part in 10⁹ at these angles, so a_horiz(t) ≈ g·b_gt. Note the shape: a fictitious horizontal acceleration that ramps linearly in time. Not a step, a ramp — that is why the answer is cubic and not quadratic.
4. Integrate once for velocity. ∫₀^t g b_gτ dτ = v(t) = g b_gt²/2.
5. Integrate again for position. ∫₀^t g b_gτ²/2 dτ = p(t) = g b_gt³/6. Then say the family out loud, because the comparison is the point: white noise on the gyro gives t^1.5, accelerometer bias gives t², gyro bias gives t³. The cubic always wins eventually, which is why the aiding interval — not the noise density — is what sizes an inertial coast.

The invert is the real question: given a position budget σ_p, the coast time is t = (6σ_p/(g b_g))^1/3. Because it is a cube root, a tenfold better gyro buys you only 2.15× more coasting. That single sentence is worth more than any amount of vocabulary about MEMS grades.

Derivation 3 — why precisions (not variances) add when you fuse. Write Σ_f⁻¹ = Σ₁⁻¹ + Σ₂⁻¹ and ask where it comes from. Two independent Gaussian measurements of the same state x.

1. Write the two log-likelihoods. Dropping constants that do not depend on x, log p₁(x) = −½(x − μ₁)^TΣ₁⁻¹(x − μ₁) and likewise for sensor 2.
2. Independence means the joint is a product, so the logs add. log p(x) = −½[(x − μ₁)^TΣ₁⁻¹(x − μ₁) + (x − μ₂)^TΣ₂⁻¹(x − μ₂)]. Everything after this is algebra on that one line.
3. Expand and collect by power of x. The quadratic terms give x^T(Σ₁⁻¹ + Σ₂⁻¹)x; the linear terms give −2x^T(Σ₁⁻¹μ₁ + Σ₂⁻¹μ₂); the rest is constant in x. So log p(x) = −½[x^TYx − 2x^Ty] + c with Y ≡ Σ₁⁻¹ + Σ₂⁻¹ and y ≡ Σ₁⁻¹μ₁ + Σ₂⁻¹μ₂.
4. Complete the square. x^TYx − 2x^Ty = (x − Y⁻¹y)^TY(x − Y⁻¹y) − y^TY⁻¹y. The trailing term does not involve x, so it folds into the constant.
5. Read off the result. What remains is a Gaussian in x with precision Y and mean Y⁻¹y — that is, Σ_f⁻¹ = Σ₁⁻¹ + Σ₂⁻¹ and μ_f = Σ_f(Σ₁⁻¹μ₁ + Σ₂⁻¹μ₂). Y and y are exactly the two accumulators in coding drill 4 below; the derivation is the code.

The natural follow-up is "and what breaks it?" — step 2. The product-of-likelihoods step is where independence enters, and it is the only place it enters, which is why correlation silently destroys the gain and why covariance intersection replaces the two unit weights with w and 1 − w that sum to one.

1 · The cheat sheet

2 · System-design patterns

Prompt A: "Specify the sensor suite for a warehouse AMR that must drive 1.2 m aisles at 1.5 m/s and never touch a shelf."

Framework. Start from the requirement, not the catalogue, and do the arithmetic in the open so every step can be audited.

Step 1 — turn the geometry into a sigma. The chassis is 0.70 m wide in a 1.20 m aisle. Clearance per side = (1.20 − 0.70)/2 = 0.50/2 = 0.25 m. That is the distance at which the robot touches a shelf, so it is a 100% failure, not a budget; divide it down to a sigma: budget σ = 0.25/3 = 0.0833 m. Then volunteer the modelling choice before you are asked: we use a 1-D 3σ factor because the constraint is lateral only and the aisle walls are parallel — the along-track error does not touch anything. If you wanted a 2-D 99.7% containment radius instead, the Rayleigh factor is 3.44 rather than 3.00, giving 0.25/3.44 = 0.073 m. That does not change the sensor decision, but it is the correction to volunteer before anyone asks.

Step 2 — go sensor by sensor and ask which delivers 8 cm, and in which direction. LiDAR at 3 cm does, easily, until it meets glass. Cameras deliver it laterally against textured racking and not at all against shrink-wrap in the dark. Wheels deliver continuity and drift systematically. Then ask the second question — what happens when each one fails — and the answer is the hazard matrix: cameras for the shrink-wrapped pallets and the mirror surfaces, wheels and IMU for continuity, no GNSS because there is no sky.

Step 3 — convert the budget into a time, because that is what actually sizes the suite. Inverting derivation 2, the inertial coast that fits inside the budget is t = (6σ_p/(g·b_g))^1/3. At consumer gyro grade, b_g = 30°/hr = 1.45×10^-4 rad/s, so t = (6 × 0.083/(9.81 × 1.45×10^-4))^1/3 = (0.498/1.423×10^-3)^1/3 = 350^1/3 = 7.0 s. So any hazard that lasts longer than seven seconds needs a survivor — a sensor that still observes lateral position while the others are blind — and the length of the longest glass bay in the building, divided by 1.5 m/s, is the number that decides whether you buy one. Name a number, then name what it is a budget for.

Prompt B: "Design the perception front end. Where does de-skew go and what does it need?"

Framework. Draw the path and defend the ordering. Assembly, then de-skew, then filtering, then registration — de-skew before voxel downsampling, because downsampling a distorted cloud averages points from different poses and bakes the error in. De-skew needs a twist source spanning the sweep, which rules out the estimator's own output; the IMU at 200 Hz is the answer, and 100 ms of gyro integration at consumer bias contributes 0.001°. Put numbers on the latency: 100 ms of sweep accumulation dominates everything else in the chain, which is why sub-sweep slices at 40 Hz are worth considering. Then volunteer the failure: if de-skew is referenced to the wrong end of the sweep, the walls straighten and the pose lags by speed × sweep period.

Prompt C: "Our depth camera works indoors and returns nothing in the yard. Fix it."

Framework. Compute the contrast budget out loud, both sides, with the multiplications shown.

Ambient side. Solar spectral irradiance at 850 nm is about 0.95 W/m²/nm at the ground. A narrowband interference filter in front of the sensor passes 30 nm of it. So the background the sensor integrates is 0.95 × 30 = 28.5 W/m².

Projector side. Assume 1.0 W of optical output — we assume 1 W because that is roughly the Class-1 eye-safe ceiling for an 850 nm diffuse projector at this aperture, so it is an engineering choice, not a datasheet lookup, and saying so is the point. Spread it over a 60° × 45° field of view at 2 m. The illuminated patch is 2 × 2 × tan(30°) = 2 × 2 × 0.5774 = 2.31 m wide by 2 × 2 × tan(22.5°) = 2 × 2 × 0.4142 = 1.66 m tall, an area of 2.31 × 1.66 = 3.83 m². Irradiance from the projector is therefore 1.0/3.83 = 0.26 W/m².

The ratio is the whole answer. 0.26/28.5 = 0.0092 = 0.9% modulation — the signal you are trying to demodulate is under one percent of the light already on the sensor, and it falls as 1/R² while the sun does not fall at all. Then state that no exposure setting fixes a signal under the background shot noise, and name the only three levers: more projector power (eye safety), a narrower filter (emitter linewidth and thermal drift), or a pulsed projector with a short exposure so the ambient integrates for less time. Finish with the architectural answer: outdoors you want passive stereo with a longer baseline, because sunlight is texture rather than interference for a passive sensor.

Prompt D: "Justify keeping a second modality when a vision-only stack is cheaper."

Framework. Refuse the framing and supply a criterion. The question is whether any single hazard removes a spanning set of your state space. Build the hazard matrix; find the rows where camera and stereo both go to zero — darkness, direct sun, shrink-wrap, glass — and ask what observes position in those rows. If the answer is "the IMU, for about nine seconds", you have your justification, and it is a number rather than an opinion. Then name what would change your mind: a measured per-hazard failure rate on this site, not a benchmark score.

3 · Coding drills

Drill 1 — undistort a pixel with no library.

python
# in:  u, v      float pixels (one point, scalar)
#      K         fx, fy, cx, cy — floats, pixels
#      dist      k1, k2, p1, p2, k3 — floats, dimensionless
# out: (x, y)    float normalised camera coords, dimensionless (Z = 1 plane)
# worked: u,v = (1180, 300), fx = fy = 600, cx = 640, cy = 360,
#         k1 = -0.28, k2 = 0.09, k3 = p1 = p2 = 0
#   xd, yd = (1180-640)/600, (300-360)/600 = (0.9000, -0.1000)
#   pass 1 -> ( 1.08314, -0.12035)   pass 2 -> ( 1.13293, -0.12588)
#   pass 3 -> ( 1.14195, -0.12688)   pass 4 -> ( 1.14328, -0.12703)
#   pass 5 -> ( 1.14347, -0.12705)   converged: x = 1.14350, y = -0.12706
#   check: re-apply the forward model to (1.14350, -0.12706) -> (0.9, -0.1) exactly
x = xd = (u - cx) / fx;  y = yd = (v - cy) / fy
for _ in range(10):
    r2 = x*x + y*y
    rad = 1 + k1*r2 + k2*r2**2 + k3*r2**3
    dx = 2*p1*x*y + p2*(r2 + 2*x*x)
    dy = p1*(r2 + 2*y*y) + 2*p2*x*y
    x, y = (xd - dx)/rad, (yd - dy)/rad

What to say while writing: "K comes off first because distortion is defined in normalised coordinates. There is no closed form, so this is a contraction mapping seeded at the distorted point; it gains about a digit per pass and ten is generous. It will stop contracting for a strongly wide-angle lens at the corners, which is when I would switch to Kannala-Brandt."

Drill 2 — de-skew a scan.

python
# in:  rng, az, t   float32[N], N ≈ 24000 points per 100 ms sweep at 10 Hz
#                   rng metres, az radians, t seconds (per-point firing time)
#      t_end        float seconds — timestamp of the LAST firing in the sweep
#      w            float rad/s (yaw rate),  v float m/s (body-x speed)
# out: X, Y        float32[N] metres, expressed in the SWEEP-END body frame
# assumes: planar yaw-only rotation, constant twist over the sweep, and v
#          along body-x (lateral velocity dropped). All three are stated out
#          loud in review — each is a real error term, not a formality.
dt = t - t_end                       # NEGATIVE — reference is the sweep END
th = w * dt
px, py = rng*np.cos(az), rng*np.sin(az)
X = np.cos(th)*px - np.sin(th)*py + v*dt   # small-angle translation
Y = np.sin(th)*px + np.cos(th)*py

What to say: "Constant twist is an approximation; in production I interpolate the IMU trajectory and use the exact SE(3) exponential, whose translation term differs from v·dt by about 2 mm at 60°/s over 50 ms. The bit I would flag in review is the sign — a straight wall proves nothing, because the wrong reference frame also produces a straight wall."

Drill 3 — block-match one row with rejection.

python
d0 = int(cost.argmin())
second = cost[np.abs(np.arange(len(cost)) - d0) > 2].min()
if second < 1.15 * cost[d0]: return np.nan        # repetitive texture
curv = cost[d0-1] - 2*cost[d0] + cost[d0+1]
if curv <= 0: return np.nan                        # no texture
return d0 - 0.5*(cost[d0+1] - cost[d0-1]) / curv    # sub-pixel

What to say: "Reject before refining. The two tests measure different failures — the ratio catches repetition, the curvature catches absence of texture — and the sub-pixel parabola matters more than it looks: at 3.5 m one pixel of disparity is 14 cm."

Drill 4 — fuse N heterogeneous estimates.

python
Y = np.zeros((n, n)); y = np.zeros(n)
for mu, S, ok in zip(means, covs, valid):
    if not ok: continue              # degraded contributes ZERO, not a guess
    Oi = np.linalg.inv(S); Y += Oi; y += Oi @ mu
P = np.linalg.inv(Y); x = P @ y

What to say: "Information adds, so fusion is a sum. One inversion at the end, and in production I keep the state in information form so there is none in the loop. The validity mask is the safety story — a bad sensor must contribute nothing, because inflating its covariance still lets a wrong mean pull the estimate. And if I cannot argue independence, I use covariance intersection and pay twice the ellipse area for consistency."

4 · Debugging scenarios

5 · Classical versus modern, and when to use which

6 · Recommended reading

The one book. Paul Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (2nd edition, Artech House, 2013). It is the only book that treats every sensor in this lesson with the same rigour and the same error-model vocabulary, and its chapters on GNSS error sources and INS error propagation are the reference for two of the six chapters here. If you read one thing on sensing, read its error-model chapters.

Five papers, and why each one.

1. Zhang, "A Flexible New Technique for Camera Calibration" (TPAMI 2000). The procedure everyone runs. Read it for what the optimiser is actually estimating and from what evidence, which is precisely what the corner-coverage failure is about.
2. Hirschmüller, "Stereo Processing by Semiglobal Matching and Mutual Information" (TPAMI 2008). Read it for the structure of the cost volume and the aggregation paths — that is where the 28 GB/s comes from, and it is the mental model you need to reason about any stereo implementation, learned or not.
3. Zhang & Singh, "LOAM: Lidar Odometry and Mapping in Real-time" (RSS 2014). Read it for the ring-based feature extraction and the two-rate architecture, then read Dellenbach et al., CT-ICP (ICRA 2022) immediately after to see how the de-skew assumption got absorbed into the estimator.
4. Forster, Carlone, Dellaert & Scaramuzza, "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry" (IEEE T-RO 2017). Read it for why the IMU is the hub of the whole estimator and how thousands of samples become one factor. This is the paper behind every VIO system you will be asked about.
5. Julier & Uhlmann, "A Non-divergent Estimation Algorithm in the Presence of Unknown Correlations" (ACC 1997). Short, and it changes how you think about every fusion diagram you have ever drawn. Read it for the argument that consistency under unknown correlation is worth paying for.

Five repositories, and exactly what to look at.

1. ethz-asl/kalibr. Look at the camera-IMU calibration model and specifically at how the time offset and the rolling-shutter line delay are estimated as parameters rather than assumed. It is the cleanest existing answer to Chapter 1's second failure mode.
2. opencv/opencv — modules/calib3d/src/undistort.dispatch.cpp. Look at the fixed-point loop in undistortPoints, count the iterations, and find the divergence guard. Then compare it with the code you wrote in Chapter 1's lab.
3. hku-mars/FAST_LIO — include/IMU_Processing.hpp. Look at the backward-propagation de-skew: how the per-point timestamps are used, and which end of the sweep is the reference. This is Chapter 3's lab in production form.
4. tomeko/allantools (or the original allantools) — look at oadev and understand why the overlapping estimator has lower variance than the non-overlapping one for the same data. Then run it on two hours from your own IMU.
5. tomojitakasu/RTKLIB — src/lambda.c and src/rtkpos.c. Look at where the ratio test is computed and where the threshold is applied. That single constant is the difference between the RTK failure in Chapter 5 shipping and being caught.

7 · The numbers drill

One significant figure, out loud, in under ten seconds each. These are the numbers that settle design arguments before anyone opens a datasheet.

Want to test the retrieval speed for real? The Studio button at the top of this lesson runs a timed practice session on exactly this material.

"What I cannot create, I do not understand."
— and what I cannot break, I do not trust.

Bridges from this lesson: Camera Models & Calibration · Sensors: LiDAR, IMU & ICP · IMU & Inertial Navigation · INS/GNSS Coupling · Time & Sync · Uncertainty & Robust Costs · Modern VIO