Time, Clocks &
Sensor Alignment

Chapter 1: Clock Domains, PTP, NTP & Hardware Triggering

Draw a box on the whiteboard, label it "camera". Draw another, label it "IMU". Now ask the only question that matters: "Do these two share a clock?"

The answer that ends the conversation early is "yes, they are both on the robot." The answer that opens it up is: "Physically, no — there are at least three independent oscillators in this rig and probably five. The real question is which ones are disciplined to a common reference, how tightly, and what the residual is."

The concept

A clock domain is a set of timestamps produced by counting the same physical oscillator. Two timestamps from the same domain are directly comparable. Two timestamps from different domains are not comparable at all until you know the mapping between them — and that mapping has two parameters, not one.

Model a clock as a function that turns true time t into a reading C(t). To first order every real clock obeys:

b is the offset, in seconds: how far the reading is from truth right now. ε is the rate error or skew, dimensionless, and always quoted in parts per million because the numbers are otherwise all zeros: a 50 ppm crystal has ε = 50 × 10⁻⁶ = 0.00005.

Now compare two clocks, A and B, in different domains:

That is the entire theory of clock synchronisation in one line. The gap between two clocks is a straight line in time: a constant term you can measure once, plus a slope you have to keep correcting. Every protocol in this chapter exists to estimate one or both of those two numbers.

Read that example again, because it contains the single most important structural fact in this chapter: a rate error is not a bug you can calibrate away once. Measure the offset at boot, store 2.76 ms in a file, and forty minutes later the file is wrong by 108 ms. Any architecture that treats sync as a one-shot factory step is broken by physics.

Then run the example backwards, because that is the form the question usually arrives in. You are not asked "how much drift in an hour"; you are asked "we have a 3 ms alignment spec — how long does this rig hold it?" Divide instead of multiply:

One more reading of the same number, because it is the one that unlocks the Debugging section. The slope is the diagnostic. If you log the difference between two clocks and it is a straight line through the origin, you are looking at ε_rel directly: read the rise over the run and you have the ppm. If it is a straight line with a jump in it, a daemon stepped. If it is flat but noisy, you are looking at measurement noise on a disciplined clock. Three shapes, three completely different bugs, one plot.

The right column is the design conversation. Nobody puts a rubidium standard on a sidewalk robot. Instead you put a cheap oscillator everywhere and discipline it — run a servo that continuously nudges its offset and its rate toward a reference. The protocols below are how the servo gets its error signal.

The one place an expensive oscillator still earns its price is holdover: the interval during which a clock must keep time after its reference disappears. A grandmaster steered by GNSS is only as good as its sky view, and a robot drives into tunnels, under bridges, and between buildings. When the satellites go away the grandmaster stops being disciplined and starts being just an oscillator again — the same straight line from the top of this section, restarted from zero.

Two consequences worth knowing. First, holdover is why the PTP wire protocol carries a clockClass field: a GNSS-locked grandmaster advertises class 6, and the instant it loses lock it re-advertises as class 7, "holdover". The number is not cosmetic — the Best Master Clock Algorithm uses it to decide whether some other node should take over. Second, holdover degradation is graceful and announced, which makes it the easiest of all the failure modes in this chapter to detect, and therefore the least likely to be the answer when something is quietly wrong.

NTP: two round trips, one assumption

NTP (Network Time Protocol) estimates the offset between a client and a server using four timestamps from one exchange. The derivation is short enough to do at a whiteboard, and worth doing because it is the smallest complete example of a measurement model with an unobservable nuisance parameter.

Let θ be the true offset (server clock minus client clock). Then the outbound leg and the return leg give:

Two equations, three unknowns: θ, d_out, d_back. The system is underdetermined, and no amount of extra exchanges fixes that — each new exchange adds two more unknowns with two more equations. NTP closes the gap with one assumption, and it is the only assumption in the protocol:

With symmetry assumed, subtract the two equations to eliminate the delay, and add them to eliminate the offset. Do it slowly, because the two directions are easy to swap under pressure and the whole point of this derivation is that you can produce it at a whiteboard. First rewrite each equation as a difference, so both legs have the same shape:

Look at the signs before you touch the algebra, because they tell you which operation does what. θ appears with opposite signs, so subtracting the two lines reinforces it and adding them kills it. The delays appear with the same sign, so it is the other way round for them. Subtract first:

The delay term has not vanished — it has been reduced to a difference. That is where the assumption enters, and it is the only place it enters: if d_out = d_back the parenthesis is zero and what is left is exactly twice the offset.

That is already the answer. The only remaining step is cosmetic: −(t₄ − t₃) is the same thing as (t₃ − t₄), which is how RFC 5905 and every implementation write it, so match the two forms in your head once and you will never mis-transcribe it:

Now go back to the same two difference equations and add them instead. This time it is θ that cancels, and notice that this cancellation needs no assumption at all — the ±θ simply annihilates:

Regroup those same four stamps and you get the form that is printed everywhere. Expand both sides to check it rather than trusting the shape: the left is t₂ − t₁ + t₄ − t₃, and the right is t₄ − t₁ − t₃ + t₂. Same four terms, same four signs.

Read the right-hand form out loud when you write it: total elapsed on the client's clock, minus the interval the server admits it spent holding the packet. Both brackets are differences taken within a single clock domain, which is why δ is immune to the offset — a constant added to both t₂ and t₃ cancels inside its own bracket.

Generalise those two examples and you get the error law of NTP, which is worth memorising because it explains every architectural decision that follows:

Derive that in one line rather than quoting it, since you already have the pieces. Substitute the true relations t₂ − t₁ = θ + d_out and t₃ − t₄ = θ − d_back straight into the estimator:

Now bound it, which takes two more lines and is the part most people skip. Both legs are physical delays, so both are non-negative, and by the exact result above they sum to the measured δ. That pins d_back = δ − d_out with d_out ∈ [0, δ]. Substitute and the two-variable expression collapses to one variable:

As d_out sweeps its entire feasible range from 0 to δ, that expression sweeps linearly from −δ/2 to +δ/2, and nothing in the protocol narrows it, because d_out is never observed. Check it against Worked example 3: there d_out = 18.0 and δ = 24.8, so the predicted error is 18.0 − 12.4 = +5.6 ms — exactly the number the estimator produced, from a formula that never saw the congestion.

This is why NTP is a fine way to keep log files roughly in order and a poor way to align a camera to an IMU. Its accuracy is not limited by its arithmetic; it is limited by an assumption about a network you do not control. Google's Spanner (Corbett et al., OSDI 2012) took the opposite and more honest approach for exactly this reason: its TrueTime API returns an interval [earliest, latest] instead of a point, and the database is built to wait out the uncertainty rather than pretend it is zero.

PTP: move the timestamp closer to the wire

PTP (Precision Time Protocol, IEEE 1588) uses the same four-timestamp exchange and the same symmetry assumption. It is three to four orders of magnitude better than NTP, and the reason has nothing to do with the mathematics.

The dominant error in software NTP is not path asymmetry on a switched LAN — it is the stack. When a userspace daemon calls clock_gettime() after recvmsg() returns, the timestamp includes the kernel's interrupt latency, the scheduler's wake-up delay, and whatever else the CPU was doing. On a loaded non-realtime Linux box that is tens to hundreds of microseconds, and it is variable, which means it does not average out into a calibratable constant.

PTP's insight: put the timestamping hardware in the network interface itself. A PTP-capable NIC has its own free-running counter (the PHC, PTP Hardware Clock) and latches it the instant the packet's start-of-frame delimiter crosses the MAC layer. The software stack is now entirely outside the measurement.

The transparent clock is the mechanism worth being able to explain, because it is where PTP stops being "NTP but faster" and becomes a different idea. A PTP-aware switch measures how long each sync packet actually sat inside it — the residence time — and writes that number into a correction field in the packet itself. The receiver subtracts it. Queueing delay, the thing that destroys NTP, is now measured rather than assumed away.

Hardware triggering: deleting the question

Every protocol above estimates the relationship between clock domains. Hardware triggering does something categorically different: it makes the sensors sample at the same instant, so there is no relationship left to estimate.

A single line — a 3.3 V pulse from an FPGA or a microcontroller — fans out to the trigger input of every camera and to a latch input on the IMU. When the edge arrives, every camera opens its shutter and the IMU's current sample index is captured into a register alongside the FPGA's own counter value. There is now exactly one clock in the system that matters, and it is the FPGA's.

That factor of two million is the argument. The natural objection is "why not just use PTP everywhere?" — and the answer is that PTP aligns clocks while a trigger aligns captures, and only the second one removes the exposure-timing and free-running-phase problems as well.

The design

Here is the rig to architect, with real rates and real byte counts. This is a mid-size delivery robot, and the numbers are the ones you should be prepared to defend in a design review.

Total: ~133 MB/s. Check the arithmetic rather than quoting the sum, because the camera term dominates by two orders of magnitude and that is itself the finding. One frame is 1280 × 800 × 1 byte = 1,024,000 B = 1.02 MB. Four cameras at 30 Hz: 4 × 30 × 1.024 = 122.9 MB/s. LiDAR: 600,000 pt/s × 16 B = 9.6 MB/s. Everything else — IMU 400 × 28 = 11.2 kB/s, encoders 200 × 16 = 3.2 kB/s, GNSS 10 × 200 = 2 kB/s — sums to 16 kB/s, which is 0.01% of the total. The whole bandwidth problem is the cameras.

Now turn bandwidth into latency, because that is the conversion the sync argument needs. Put the four cameras on two USB3 host controllers, two each. A 5 Gb/s USB3 link does not deliver 5 Gb/s of payload: 8b/10b line coding takes 20% (→ 4.0 Gb/s = 500 MB/s), and USB3 bulk framing plus UVC headers take roughly another quarter, leaving about 375 MB/s of usable payload. One frame alone on an idle controller therefore takes 1.024 / 375 = 2.73 ms.

But the cameras are hardware-triggered, so they are never staggered — both frames on a controller are generated in the same instant and the second one queues behind the first. Its bytes finish at 2 × 2.73 = 5.46 ms, and a pipeline that waits for all four views must budget the last arrival, not the average. That is the 5.5 ms in the link-transfer row below; it is a derived number, not a measured one.

The same conversion is the argument against moving cameras onto the shared 10 GbE spine "because there is room". There is: 133 MB/s of 1180 MB/s is 11%. But now all four frames contend with the LiDAR's 9.6 MB/s and with each other in one queue, and the last frame lands at 4 × 1.024 / 1180 = 3.5 ms plus whatever the LiDAR inserted. Faster on paper, and coupled to a stream you do not control. Every megabyte you move is milliseconds of the offset you are trying to eliminate, and every shared link is a stage whose jitter you cannot subtract.

The daemon stack, and the piece everyone forgets. On Linux the chain has three independent links, and an engineer who names all three is immediately distinguishable from one who says "we run PTP":

Latency budget, stage by stage, for the camera path on this rig. This is the table a design review runs on. The point is not the exact numbers — it is that you know which stages have jitter and which do not.

Reading the table is the skill. The "calibratable?" column is doing all the work, and it splits every row into two independent questions: can I subtract the mean? and can I subtract this frame's deviation from that mean? A stage whose mean is fixed and known contributes a constant lag you remove with one subtraction and forget. A stage whose per-frame value you cannot observe contributes noise that survives every correction you will ever apply. Sum the first kind to get the bias; combine the second kind to get the floor.

That contrast is the reason the stamping site matters more than the protocol. Perfect PTP with a 100 ns lock buys you nothing if the number you then write into header.stamp was measured after 3.33 ms of scheduler noise — and on the driver that hides its exposure, that same 100 ns lock is 96,000 times smaller than the 9.6 ms of noise sitting on top of it (9.6 × 10⁻³ / 100 × 10⁻⁹ = 96,000). Sync the clocks, then move the stamp, then interrogate the driver. In that order, because each step is cheaper than the one after it and the last one recovers the most.

The code

Two implementations, both short, both worth being able to produce on demand. First the NTP offset estimator, exactly as derived above, with the honest uncertainty interval attached.

python
import numpy as np

def ntp_offset(t1, t2, t3, t4):
    """One NTP exchange. t1,t4 on the client clock; t2,t3 on the server clock.
    Returns (offset_estimate, round_trip, worst_case_error).
    offset is (server - client): ADD it to a client stamp to get server time."""
    theta = 0.5 * ((t2 - t1) + (t3 - t4))   # symmetric-path estimate
    delta = (t4 - t1) - (t3 - t2)               # round trip, minus server think time
    return theta, delta, delta / 2.0          # the bound is HALF the round trip

# Healthy exchange (ms)
print(ntp_offset(0.0, 12.2, 15.5, 28.1))   # (-0.2, 24.8, 12.4)
# True offset zero, but the outbound leg is congested
print(ntp_offset(0.0, 18.0, 21.3, 28.1))   # (+5.6, 24.8, 12.4)  <- same delta!

The second is the one you would actually ship: a least-squares fit of both clock parameters at once, so you recover the rate as well as the offset. This is what a disciplining servo is doing underneath, and it is four lines.

python
def fit_clock(t_ref, t_dev):
    """Paired observations of the same instants on two clocks.
    Model:  t_dev = (1 + eps) * t_ref + b
    Returns (offset_seconds_at_t_ref_zero, skew_ppm)."""
    A = np.column_stack([t_ref, np.ones_like(t_ref)])   # (N, 2)
    slope, b = np.linalg.lstsq(A, t_dev, rcond=None)[0]
    return b, (slope - 1.0) * 1e6                       # ppm

# 20 minutes of PPS-vs-host-clock pairs, 1 Hz
t_ref = np.arange(1200, dtype=float)
t_dev = t_ref * (1 + 46e-6) + 0.0031 + np.random.default_rng(0).normal(0, 2e-5, 1200)
b, ppm = fit_clock(t_ref, t_dev)
print("offset %.4f ms, skew %.2f ppm" % (b * 1e3, ppm))  # ~3.10 ms, ~46.0 ppm

The third one is the one that actually runs inside ptp4l and phc2sys, and almost nobody can produce it from scratch. A batch least-squares fit tells you what the clock did; a servo decides what to do about it, once per second, forever, with no memory of the whole history. linuxptp's default is a plain PI controller: proportional term for the offset in front of you, integral term to absorb the constant rate error.

python
def pi_servo(offsets_ns, kp=0.7, ki=0.3):
    """linuxptp's clock servo. offsets_ns[k] = measured (device - reference)
    at the end of interval k, one sample per second. Yields the frequency
    correction in ppb to program via clock_adjtime() for the next interval.
    Units work out for free at a 1 s interval: nanoseconds per second IS ppb."""
    drift = 0.0                     # the integrator
    for o in offsets_ns:
        drift += ki * o             # accumulates toward the crystal's TRUE rate error
        yield kp * o + drift        # ppb to apply right now

# Closed loop against a crystal that runs 46 ppm fast, no phase steps (slew only)
TRUE_PPB = 46.0 * 1000.0              # 46 ppm = 46000 ppb = 46000 ns gained per second
o, drift, corr = 0.0, 0.0, 0.0
for k in range(1, 8):
    o    += TRUE_PPB - corr         # ns gained this second = true rate minus what we applied
    drift += 0.3 * o
    corr  = 0.7 * o + drift
    print("t=%ds  offset %7.0f ns   integrator %7.0f ppb   correction %7.0f ppb" % (k, o, drift, corr))
# t=1s  offset   46000 ns   integrator   13800 ppb   correction   46000 ppb
# t=3s  offset   32200 ns   integrator   37260 ppb   correction   59800 ppb
# t=7s  offset     598 ns   integrator   46547 ppb   correction   46966 ppb

The production form. On a real robot you do not write any of this; you configure it and then monitor it.

Debugging

Four failure modes, all of which have taken down real fleets, each with a metric that names it unambiguously. Two of them — F1 and F2 — carry numbers because the rest of the lesson refers back to them by number; the other two are named. Three of the four present identically at the first report: "the sensors are misaligned and it got worse over the shift." The whole diagnostic skill is telling them apart, and you do it before touching a config file, by looking at the shape of the error-versus-time plot:

Commit that table to memory in preference to the four tables below it. When a symptom lands in your queue, what you ask for first matters most; "can I see offset plotted against uptime?" is the question that collapses four candidate causes to one, and it costs nothing to ask.

The frontier

Three things are genuinely moving here, and each deserves one specific paper or standard you can name.

Practice

Chapter 2: Capture Time vs Receive Time — Where the Stamp Actually Lands

Chapter 1 got every clock in the robot agreeing to within a hundred nanoseconds. This chapter is about the fact that it did not help, because the number written into the message was never a measurement of when the data was captured.

The concept

Define two quantities precisely, because the whole chapter is the gap between them.

L is the latency: the mean, repeatable part of the pipeline. η is the jitter: the zero-mean random part. This split is the most useful decomposition in the chapter, because the two halves have completely different fates.

Why the middle of the exposure, and not the start

A camera does not sample the world at an instant. It integrates over a window. If the shutter opens at t₀ and closes at t₀ + T, the pixel value is:

where E is the irradiance arriving at that pixel. So what single instant should we attribute the image to?

Take a point feature moving across the sensor at a constant image velocity. During the exposure it smears into a streak. The intensity along that streak is uniform, because the feature spent equal time at every position. Any reasonable corner or centroid detector will localise the streak at its centroid, and the centroid of a uniform segment is its midpoint. So:

That is the mid-exposure convention, and it is a derived result, not a convention someone picked. It is exact for constant velocity, and its error for accelerating motion is the second-order term we already showed is 283 times smaller.

It is worth doing that error bound properly, because it is the one place in this chapter where you can be too careful. Under acceleration the streak is no longer uniform — the feature dwells longer where it moves slower — but the intensity deposited at a position is still proportional to the time spent there, so the streak's centroid is exactly the time-average of position over the window. Expand x(t) = x_mid + v·s + ½a·s² about the midpoint, with s running over [−T/2, +T/2]. The linear term averages to zero by symmetry. The quadratic does not: the mean of s² over that window is T²/12, so

Rolling shutter: one image, one thousand capture times

A rolling shutter sensor does not expose all rows at once. It sweeps the exposure window down the sensor, so row r starts its exposure later than row 0 by a fixed line time. Over a sensor of H rows with total readout time T_ro:

This is why a single header.stamp on a rolling-shutter image is a lie by construction, and why the honest fix is to carry the line time in CameraInfo and let the estimator evaluate the pose per row. The other fix, which is the one shipped by every serious robotics rig, is to buy global-shutter sensors.

The IMU FIFO: one packet, thirty-two different instants

A 400 Hz IMU raises 400 interrupts per second, and 400 interrupts per second on a small CPU is real load. So the sensor buffers. It fills an internal FIFO and hands up a batch when the batch is full or a timer fires.

Now a driver receives one packet at time t_rx containing 32 samples, and every naive implementation ever written does the obvious thing: it stamps them all with now(). Consider what that claims.

Worse than the magnitude is the shape. Because every packet has the same internal structure, the error is not random — it is a sawtooth, ramping from 80.6 ms down to 3.1 ms and jumping back, 12.5 times per second. Integrate a gyro over that and you accumulate a rotation bias in a fixed direction. To an IMU pre-integration front-end this looks exactly like a gyro bias, and the estimator will happily "correct" a bias that is actually a timestamping error — which means the bias estimate goes wrong too, and now two states are lying.

Five places you could put the stamp

This is the table to draw on the whiteboard. Every row is a real option someone has shipped; the last column is the one that decides.

Read the mean column and options C, D and E look like a 2× spread — annoying but survivable, and all three are calibratable. Read the jitter column and the picture changes completely: 0.3, 0.9, 3.1. That is a 10× range in the quantity you can never remove.

Those five numbers are not five measurements — they are one ladder, read at five heights. Every row is a prefix sum of the same pipeline, so if any single value is challenged you can rebuild all of them. Take the default rig from the Practice widget below: exposure T = 8 ms, readout 2.0 ms, transfer 5.5 ms, driver work 1.5 ms, executor callback 2.0 ms. Start the clock at the true capture instant, which is mid-exposure.

Check it against the widget: at its default T = 8 ms the last two prefix sums are exactly 13.0 and 15.0, the same numbers as rows D and E. (The widget draws the 1.5 ms of driver work after the transfer rather than before it; addition commutes, so the totals that define rows D and E are identical either way.) Being able to say "fifteen milliseconds is four plus two plus one and a half plus five and a half plus two, and here is what each term is" is the difference between quoting a table and owning one.

The design

What ROS 2 actually gives you. There are three distinct times attached to a message and they mean three different things. Conflate them and the bug reports write themselves.

The driver contract. When you review a new sensor driver, this is the checklist. Any "no" is a change request, not a nitpick.

Memory cost of doing it right. Correct capture-time stamping forces you to buffer, because you cannot align a stream to an instant you have not yet received data for. Sizing that buffer is a design-round question with an arithmetic answer, and "200 ms" is not a round number somebody liked. It is the worst-case query lag — how far into the past your newest consumer will ever reach — times a safety factor. Build it one term at a time, using only numbers already on this page.

The code

Reconstructing a capture time is arithmetic, but arithmetic with a lot of places to put a sign backwards. Here it is written the way it should be reviewed — every term named, every term logged.

python
import numpy as np

def camera_capture_time(t_rx, exposure_s, readout_s, transfer_s, driver_s,
                         row=None, rows=None):
    """Reconstruct the capture instant from the receive instant.
    Every term is a MEAN; the jitter around them is your accuracy floor."""
    # Walk the pipeline BACKWARDS from arrival. Every step is a subtraction.
    t_exposure_end = t_rx - driver_s - transfer_s - readout_s
    #   ^ readout BEGAN here, so integration stopped here. This is the END of
    #     the exposure window [t_exposure_end - T, t_exposure_end], not its start.
    t_cap = t_exposure_end - 0.5 * exposure_s      # mid-exposure  <- the derived bit
    if row is not None:                              # rolling shutter, per row
        # t_exposure_end belongs to the LAST row; row 0 stopped one sweep earlier.
        t_row0_exposure_end = t_exposure_end - readout_s
        t_cap = t_row0_exposure_end - 0.5 * exposure_s + row * (readout_s / rows)
    return t_cap

# The Chapter 0 camera, in a dim warehouse. Print with %.5f, not repr - see below.
t_rx = 100.0000
print("bright: %.5f" % camera_capture_time(t_rx, 0.002, 0.002, 0.0055, 0.0015))
print("dim   : %.5f" % camera_capture_time(t_rx, 0.020, 0.002, 0.0055, 0.0015))
# bright: 99.99000
# dim   : 99.98100   -> the dim frame was captured 9.0 ms EARLIER than the bright one
#   bright longhand: 100.0 - 0.0015 - 0.0055 - 0.002 - 0.001 = 99.99000
#   dim    longhand: 100.0 - 0.0015 - 0.0055 - 0.002 - 0.010 = 99.98100

# --- this function IS worked example 2, not a paraphrase of it ---------------
T_RO, H, T = 0.012, 1080, 0.005
kw = dict(exposure_s=T, readout_s=T_RO, transfer_s=0.0055, driver_s=0.0015, rows=H)
t0 = camera_capture_time(100.0, row=0, **kw) - 0.5 * T   # row 0 opened here
for r in (0, 540, 1079):
    print("row %4d -> t0 + %.3f ms" % (r, (camera_capture_time(100.0, row=r, **kw) - t0) * 1e3))
print("spread  : %.3f ms" % ((camera_capture_time(100.0, row=H - 1, **kw)
                          - camera_capture_time(100.0, row=0, **kw)) * 1e3))
# row    0 -> t0 + 2.500 ms
# row  540 -> t0 + 8.500 ms
# row 1079 -> t0 + 14.489 ms     <- the three numbers from worked example 2
# spread  : 11.989 ms            <- and its 11.989 ms of intra-frame spread

def imu_batch_capture_times(t_rx, n, fs, transport_s):
    """A burst ARRIVES together; it was not CAPTURED together."""
    newest = t_rx - transport_s
    return newest - np.arange(n - 1, -1, -1) / fs      # oldest first

ts = imu_batch_capture_times(100.0, 32, 400.0, 0.0031)
print("oldest is %.1f ms stale, newest %.1f ms" % ((100.0-ts[0])*1e3, (100.0-ts[-1])*1e3))
# oldest is 80.6 ms stale, newest 3.1 ms

The rolling-shutter branch has the same trap one level down. The anchor t_exposure_end is the instant the last row stopped integrating — that is what the arrival time tells you about. But the per-row formula from earlier in this chapter, t_cap(r) = t₀ + r·(T_ro/H) + T/2, counts rows upward from row 0. Adding row * line_time to a last-row anchor would place row 1079 after the whole frame had finished reading out, which is why the code steps back one full sweep to t_row0_exposure_end before it adds anything. If your two expressions disagree about which row is the origin, the bug is invisible at row 0 and maximal at the bottom of the image — and "the error grows down the frame" is a symptom people reliably misattribute to lens distortion.

What makes this function reviewable. Three things matter here, and none of them is Python fluency. First, the anchor is named ("t_rx is the only instant I observe"). Second, every term is a mean and the docstring says so — it is doing real work, because the function silently promises an accuracy floor equal to the jitter of the terms it subtracts. Third, the test ships with the function: a function whose output you cannot check against a hand-worked number is a function nobody should merge.

The production form is mostly about getting the sensor to tell you, so you never have to model the pipeline at all.

Debugging

Three failure modes, and all three share a structure worth naming before you read them: each one has a plausible, wrong first hypothesis that a competent team will chase for a week. Texture. Gyro bias. Wheel radius. The value you bring when you debug this is not the correct hypothesis — it is the one measurement that separates the correct hypothesis from the plausible one in an afternoon, and the discipline to take that measurement before touching any code.

Work them in that order: symptom, the decoy everyone reaches for, the discriminating metric, the cheap confirmation, and only then the fix. Reaching for the fix first proves nothing about the diagnosis; reaching for the discriminating metric first proves everything.

F3 is the one you meet first, because auto-exposure is on by default and nobody thinks of the exposure control loop as part of the timing stack. It is, and it is the only part that a hardware trigger cannot fix.

F4 is the expensive one, because its decoy is not merely wrong — it is a state the estimator will happily absorb. A timestamping error that looks like gyro bias gets fitted as gyro bias, so the filter reports healthy residuals while two of its states quietly hold each other's errors. That is the worst possible failure signature: an estimator that is confidently wrong and internally consistent. Any time an estimated calibration parameter disagrees with the same parameter measured on a bench, suspect that the estimator is absorbing a timing error, not that the bench is lying.

Notice how the two metrics below play different roles. The stamp-difference histogram proves the bug exists in thirty seconds and needs no motion, no ground truth and no second sensor. The gyro-versus-camera cross-correlation quantifies it, and its peak offset lands on a suspiciously round 40 ms — exactly half the 80.6 ms batch span. Round numbers in a measured offset are a fingerprint of structure, not of noise; noise does not land on halves.

F5 is the one the driver contract promises and most reviews forget. Item 5 of that checklist — publish the sensor's sequence number and count the gaps — reads like bookkeeping until you work out what a single dropped frame does to a pipeline that assumes frames are uniformly spaced. It does not produce noise. It produces a scale error, and scale errors are attributed to wheel radius, to monocular initialisation, to tyre pressure, to anything except the USB controller.

Do the arithmetic before reading the table, because the result is an identity worth memorising. Over a route of duration τ the robot travels v·τ. If the pipeline advances its estimate by one nominal frame period T = 1/30 s per received frame, and a fraction p of frames never arrive, then it advances only N(1−p) times, so it integrates v·τ·(1−p) metres. The trajectory comes out short by exactly the drop rate — independent of speed, independent of route, independent of the estimator.

The frontier

A frontier answer fails in two opposite ways. It is vague — "I think event cameras are interesting" — or it is a list of paper titles with no claim attached. The structure that works here has three rungs, and the three rows below are deliberately one of each: what is available today and you are not using, what deletes the problem instead of solving it, and what replaces your model of the pipeline with a measurement of it.

Rung one is the strongest of the three, because it is not speculation. A GigE Vision 2.0 camera with IEEE 1588 will hand you a capture timestamp already expressed in the robot's PTP domain, and the entire CODE section above — every subtraction, every sign you could get backwards — becomes dead code. The interesting question is therefore not "does this exist" but "why is your stack not using it", and the honest answers (the sensor predates the standard; the driver drops the chunk data; nobody enabled ChunkModeActive) are all cheap to fix relative to their payoff.

Rung two is worth understanding precisely because it dissolves the chapter's vocabulary rather than improving it. An event camera has no exposure window, so mid-exposure is undefined; no rows read in sequence, so rolling shutter is undefined; no frame, so frame latency is undefined. What survives is the buffer, the bracketing query and the interpolation — Chapter 3's machinery, running at microsecond granularity on a stream that never stops. Say that out loud and you have shown you know which of your concepts were about physics and which were about frames.

Rung three is the one that changes your Monday. The latency table earlier in this chapter is a set of estimates; ros2_tracing turns each row into a measured distribution with under 2% overhead, which means the numbers can go into CI and a regression in driver latency becomes a failing build rather than a field report six months later.

Practice

Chapter 3: Interpolation & Time-Alignment of Asynchronous Streams

Every clock now agrees to a hundred nanoseconds and every message carries an honest capture time. The robot still cannot fuse anything, because the camera samples at 30 Hz, the IMU at 400 Hz, the LiDAR at 10 Hz and the wheel encoders at 200 Hz, and no two of them ever sample at the same instant.

The concept

Formally: you hold samples of a signal x at instants {t₀, t₁, …} and you need x at a query instant t_q that is not one of them. Three families:

The first two bounds are worth deriving, because the ratio between them is the argument you will make in a design review.

Bound 1: zero-order hold

If samples are spaced h apart, the farthest a query can sit from its nearest sample is h/2. Over that gap the signal moves at rate x′, so:

Bound 2: linear interpolation

Put the interval at [0, h] and let p(t) be the straight line through the two bracketing samples:

Define the error e(t) = x(t) − p(t). By construction e(0) = e(h) = 0. Everything else follows from Rolle's theorem applied twice — this is where the ½x″(ξ) comes from, and it is worth being able to produce it, because "the standard interpolation remainder" is a phrase, not an argument.

Step 1 — build a function with three roots. Pick any query instant s strictly inside (0, h). Let w(t) = t(t − h), which vanishes at 0 and h and is non-zero at s. Now define

The scaling was chosen exactly so that g(s) = e(s) − e(s) = 0. And g(0) = g(h) = 0 because both e and w vanish there. So g has three roots in [0, h]: at 0, at s, and at h.

Step 2 — Rolle, twice. Between two roots of a differentiable function there is a root of its derivative. Three roots of g give two roots of g′ (one in (0, s), one in (s, h)); two roots of g′ give one root of g″, at some ξ strictly inside (0, h).

Step 3 — differentiate twice and read off the answer. p is a straight line so p″ = 0, and w(t) = t² − ht so w″ = 2, a constant. Therefore

Setting g″(ξ) = 0 and solving for e(s) gives e(s) = ½ x″(ξ) w(s). The instant s was arbitrary, so rename it t:

The polynomial t(t − h) is a downward parabola with roots at 0 and h, so its largest magnitude is at the midpoint t = h/2, where it equals (h/2)(−h/2) = −h²/4. Substituting:

For position, x″ is acceleration. The error no longer depends on how fast you are going at all — only on how fast that is changing.

Bound 3: the third family, and why it is not free

The first two bounds are special cases of one identity. Fit the unique polynomial of degree n through n+1 samples t₀ … t_n and the same Rolle argument, run n+1 times instead of twice, gives

Zero-order hold is n = 0 (one sample, error ∝ h). Linear is n = 1 (two samples, error ∝ h²). A cubic through four samples is n = 3 and its error is ∝ h⁴. Each extra sample in the stencil buys another power of h — provided the extra samples exist, which is the whole catch.

Production continuous-time stacks do not use the raw Lagrange cubic (it oscillates and it is only C⁰ across knots). They use a spline: a piecewise cubic stitched together with continuity conditions. The two you meet are

The cost that is easy to miss. A four-sample stencil needs a sample on each side of the bracketing pair, not just of the query. So the minimum lag doubles: linear waits for one sample past t_q, a cubic waits for two. On a 400 Hz IMU that is 2.5 ms becoming 5.0 ms. And on the first and last interval of any finite buffer the four-point stencil does not exist at all, so the scheme silently drops to second order exactly where a naive implementation clamps and pretends. This is why the widget below measures its error over the fully-bracketed span only, and why B-spline fitters pad knots beyond the data.

Rotations do not average

Position interpolates by blending numbers. Rotation does not, because rotations do not live in a vector space — the average of two unit quaternions is not a unit quaternion, and rescaling it to unit length does not put it where you wanted.

Set up the problem on the sphere. Two unit quaternions q₀ and q₁ subtend an angle Ω, with cos Ω = q₀ · q₁. We want a path that stays on the unit sphere and sweeps at a constant angular rate, so at parameter u it must sit at angle uΩ from q₀. Write the answer as a blend and impose exactly that:

Those are two linear equations in α and β, and solving them is the derivation of SLERP. Do it once by hand and you never have to memorise the formula again.

Step 1 — substitute and get the system. Dot q(u) = αq₀ + βq₁ with q₀, using q₀·q₀ = 1 (unit quaternion) and q₀·q₁ = cos Ω:

Dot the same expression with q₁, using q₁·q₁ = 1:

Step 2 — eliminate. Multiply the second equation by cos Ω and subtract it from the first. The βcosΩ terms cancel and what is left is

That leading factor is the determinant of the 2×2 system — and 1 − cos²Ω = sin²Ω. (Notice immediately where the numerical trouble will be: the determinant vanishes as Ω → 0. That is the entire reason for the small-angle fallback two tables below; it is not a hack, it is a singular linear system.)

Step 3 — one product-to-sum identity. Use cos A cos B = ½[cos(A−B) + cos(A+B)] with A = Ω and B = (1−u)Ω, so A−B = uΩ and A+B = (2−u)Ω:

Substituting, the numerator collapses:

Step 4 — one difference-to-product identity. cos X − cos Y = −2 sin((X+Y)/2) sin((X−Y)/2). With X = uΩ and Y = (2−u)Ω the half-sum is Ω and the half-difference is (u−1)Ω, so

Therefore α sin²Ω = sin Ω sin((1−u)Ω), and one factor of sin Ω cancels:

Step 5 — β for free. Swapping u ↔ 1−u swaps the two constraint equations, which swaps the roles of q₀ and q₁, so β is α with u in place of 1−u. Putting them together gives SLERP, spherical linear interpolation:

Sweep u across the whole interval and the worst NLERP error for a 90° separation is 0.919°, near u = 0.22. At a landmark 10 m away that is 10 × 0.919 × (π/180) = 160 mm of lateral error, from choosing the wrong four lines of code.

Two implementation details separate a correct SLERP from a nearly correct one:

The lag you cannot avoid

Interpolation needs a sample on both sides of the query. So to answer "what was the angular rate at t_q" you must wait until a sample arrives after t_q. That wait is real latency, and it is the price of correctness.

That is the architecture. The slowest, fattest stream provides the query timestamps; every fast, cheap stream is interpolated onto them. It falls straight out of the buffer-cost table in Chapter 2 and it is what every production VIO stack does, whether or not its authors phrase it this way.

The design

The ROS 2 recipe everyone reaches for, and why it is a trap. message_filters::sync_policies::ApproximateTime takes N topics and emits tuples whose timestamps are close together. It is well engineered, widely used, and solves a different problem than the one you have.

That third row is the number to remember: 4.25 mm versus 0.0016 mm, a factor of 2,700, between selecting and interpolating. And the fourth row is worse, because two free-running cameras with random relative phase pair with an average error of 8.3 ms and a worst case of 16.7 ms — nearly 57 mm of geometric error that the pipeline never mentions.

The architecture that is actually right. Three components, and it is the same shape whether you write it yourself or use tf2:

The code

The whole aligner, from scratch, in under forty lines — short enough to rebuild from memory when you need it.

python
import numpy as np

def bracket(ts, tq):
    """(i, u) with ts[i] <= tq <= ts[i+1]; None if unanswerable."""
    if len(ts) < 2 or tq < ts[0] or tq > ts[-1]:
        return None                       # refuse, never clamp
    i = min(np.searchsorted(ts, tq, side='right') - 1, len(ts) - 2)
    return int(i), (tq - ts[i]) / (ts[i+1] - ts[i])

def slerp(q0, q1, u):
    """Unit quaternions (w,x,y,z). Constant angular rate from q0 to q1."""
    q0 = np.asarray(q0, float); q1 = np.asarray(q1, float)
    d = float(q0 @ q1)
    if d < 0.0:                        # q and -q are the SAME rotation:
        q1, d = -q1, -d               # flip so we take the short arc
    d = min(1.0, max(-1.0, d))
    if d > 0.9995:                    # under 1.81 deg apart: sin(Om) -> 0
        q = q0 + u * (q1 - q0)        # NLERP is safe and accurate here
        return q / np.linalg.norm(q)
    Om = np.arccos(d); s = np.sin(Om)
    return (np.sin((1-u)*Om)/s) * q0 + (np.sin(u*Om)/s) * q1

def pose_at(ts, pos, quat, tq):
    """Evaluate a pose stream at an arbitrary instant. None if out of range."""
    br = bracket(ts, tq)
    if br is None:
        return None
    i, u = br
    p = pos[i] + u * (pos[i+1] - pos[i])     # vectors: linear
    q = slerp(quat[i], quat[i+1], u)         # rotations: spherical
    return p, q

Two natural follow-ups. "Make the bracket search O(1)" and "what is the third family?". Both are short, and both show up in production code.

python
class MonotoneBracket:
    """Queries from a fixed-lag aligner arrive in time order, so the binary
    search is wasted work: remember the last index and walk forward.
    O(1) amortised, and it is the version that runs in the hot path."""
    def __init__(self, ts):
        self.ts = ts
        self.i  = 0
    def __call__(self, tq):
        ts = self.ts
        if tq < ts[0] or tq > ts[-1]:
            return None                          # out of buffer: refuse
        if tq < ts[self.i]:
            self.i = 0                            # a rewind (bag replay) -> restart
        while self.i + 2 < len(ts) and ts[self.i + 1] < tq:
            self.i += 1
        i = self.i
        return i, (tq - ts[i]) / (ts[i+1] - ts[i])

def catmull_rom(ts, xs, tq):
    """The third family, cheap version: a C1 cubic THROUGH the samples.
    Needs TWO samples on each side, so it refuses one interval further in
    than lerp does -- that extra interval is the extra lag it costs."""
    if len(ts) < 4 or tq < ts[1] or tq > ts[-2]:
        return None                              # unbracketed for a 4-pt stencil
    i = int(np.searchsorted(ts, tq, side='right')) - 1
    i = min(max(i, 1), len(ts) - 3)     # keep the 4-point stencil in range
    h = ts[i+1] - ts[i]
    u = (tq - ts[i]) / h
    p0, p1, p2, p3 = xs[i-1], xs[i], xs[i+1], xs[i+2]
    return 0.5 * ((2*p1)
                  + (-p0 + p2) * u
                  + (2*p0 - 5*p1 + 4*p2 - p3) * u**2
                  + (-p0 + 3*p1 - 3*p2 + p3) * u**3)

def catmull_rom_deriv(ts, xs, tq):
    """...and its derivative in closed form -- the actual reason to want a
    continuous-time representation at all, since an IMU measures d/dt."""
    i = int(np.searchsorted(ts, tq, side='right')) - 1
    i = min(max(i, 1), len(ts) - 3)
    h = ts[i+1] - ts[i]; u = (tq - ts[i]) / h
    p0, p1, p2, p3 = xs[i-1], xs[i], xs[i+1], xs[i+2]
    return 0.5 * ((-p0 + p2)
                  + 2*(2*p0 - 5*p1 + 4*p2 - p3) * u
                  + 3*(-p0 + 3*p1 - 3*p2 + p3) * u**2) / h

Debugging

The frontier

The frontier here is the same idea taken to its conclusion: stop storing a trajectory as samples and start storing it as a function.

Practice

Chapter 4: Showcase — The Two-Sensor Alignment Bench

A delivery robot weaves down 96 m of sidewalk, swinging 1.6 m to each side every 24 m as it threads between parked scooters. At walking pace its localisation is clean. Wind it up to 3.4 m/s — same route, same code, same map, same everything — and it finishes 32 millimetres off. Two thirds of a 50 mm budget, spent on nothing that appears in any log.

That robot is on the bench below. Set the offset slider to +12 ms and the 32 mm appears; drag it back to zero and it vanishes, at every speed you can select. The rest of this chapter is the explanation of why those two sentences are both true.

The setup

The route is fixed — four lobes, 96 m of ground track, a lateral amplitude of 1.6 m over a 24 m wavelength. It is a realistic sidewalk weave around parked scooters, and it is chosen because it exercises the rotation channel hard while returning to the same heading, so nothing is hidden by a lucky cancellation.

Two sensors. The IMU is correct. The camera provides attitude, and its measurements describe the world Δt seconds before the label they carry. The estimator, believing the labels, dead-reckons along a heading that is stale by exactly that amount.

The governing equation, derived

The estimator's reported heading is stale by Δt, so it is wrong by the amount the robot turned during that interval:

Dead-reckoning at speed v along a heading that is off by δθ accumulates cross-track error at a rate of v · δθ (for small angles, sin δθ ≈ δθ):

Both v and Δt are constants here, so they come straight out of the integral, and what remains integrates to the swept heading:

The bench

Six experiments, with what you should see

Run these in order. Each one is a claim from an earlier chapter that the bench either confirms or refutes.

Where the residual floor comes from

Experiment 5 is the one that most needs its number derived rather than quoted, because "press the button and the error mostly goes away" is exactly the kind of hand-wave that collapses under one follow-up question. So: why 2.4 mm, and not zero?

Subtracting a calibrated constant removes the mean of the offset. It cannot remove the jitter, because jitter is a fresh random draw on every frame and there is no single number to subtract. Chapter 2's stamping-site table gives the figure directly: option D, a userspace driver stamping after the read returns, carries a mean of 13.0 ms and a jitter of 0.9 ms. That 0.9 ms is what the bench uses as its post-correction residual, and it is the honest floor for a team that has calibrated well but has not moved the stamp.

Reading the panel

Turning the bench into a speed limit

The single most useful thing you can do with e = v · Δt · Θ is invert it. Fix the error budget, and the equation hands you the maximum speed the robot may travel:

Those are the rotation channel alone. The along-track channel from Chapter 0 contributes another v · Δt on top, with a factor of 1 rather than 0.79, so a conservative combined budget roughly halves every number above: at 12 ms the honest limit for this route is closer to 2.9 m/s, which is exactly where the ticket in Chapter 0 said things fell apart.

What the four fixes actually cost

Sooner or later you have to choose. Here is the ladder from Chapter 0, priced.

Why the calibrated constant goes stale

Fix 2 is the cheapest and it is the one that quietly decays. Three things move the offset after you have measured it, and an engineer who names all three is describing a system they have operated.

Put those together and a constant measured at 12.0 ms on a cool morning in a bright loading bay can be 13.9 ms on a warm afternoon indoors. That 1.9 ms of residual is 6.5 mm at 3.4 m/s — acceptable, but it is the reason fix 2 needs either a recalibration cadence or fix 5 running alongside as a monitor.

Turning the bench into a regression test

The last step, and the one that keeps the bug fixed: this bench is a unit test. It needs no hardware and no recorded data.

The code

Both boxes of that flow are small enough to write at a whiteboard, and both are worth being able to produce on demand. Box 1 is the generator — the bench itself. Box 2 is the estimator — the shake test that produces the td_hat the assertion checks. Here they are, from scratch, in that order.

1. The bench integrator

Forty lines that reproduce worked example 1 exactly. Notice that the entire bug is one line — th_est = th - om*td — and that everything around it is honest kinematics with no small-angle assumption anywhere. That asymmetry is the point: a one-line defect in a hundred lines of correct code, producing an error a third of a pixel wide.

python
import numpy as np

L_SLALOM, X_END = 24.0, 96.0          # wavelength and ground track, in metres

def bench(v, td, A, N=900):
    """Peak cross-track error (m) when the reported heading is stale by td seconds.
    Returns (peak_error_m, swept_heading_rad). No small-angle assumption anywhere."""
    kw = 2*np.pi / L_SLALOM              # spatial frequency, rad per metre
    h  = (X_END / v) / N                   # integration step, seconds
    tx = ty = ex = ey = 0.0                # truth and estimate, integrated separately
    peak = 0.0
    th_hi, th_lo = -9.0, 9.0              # running max/min heading -> Theta

    for i in range(N+1):
        x     = v * (i*h)                                  # ground-frame x = v*t
        slope = A*kw*np.cos(kw*x)                       # d/dx of A*sin(kw*x)
        th    = np.arctan(slope)                       # true heading
        curv  = -A*kw*kw*np.sin(kw*x) / (1+slope**2)**1.5  # signed curvature
        sp    = v*np.sqrt(1+slope**2)                    # speed ALONG the path, not v
        om    = curv * sp                                  # yaw rate = kappa * path speed

        th_est = th - om*td            # THE BUG, in one line: a heading td seconds old

        th_hi, th_lo = max(th_hi, th), min(th_lo, th)
        tx += np.cos(th)*sp*h;      ty += np.sin(th)*sp*h       # ground truth
        ex += np.cos(th_est)*sp*h;  ey += np.sin(th_est)*sp*h   # what the estimator believes
        peak = max(peak, np.hypot(ex-tx, ey-ty))

    return peak, th_hi - th_lo

peak, Theta = bench(v=3.4, td=0.012, A=1.6)
print(f"Theta     = {Theta:.5f} rad")                  # Theta     = 0.79331 rad
print(f"simulated = {peak*1000:.1f} mm")               # simulated = 32.4 mm
print(f"predicted = {3.4*0.012*Theta*1000:.1f} mm")    # predicted = 32.4 mm

for v in (0.5, 1.4, 2.2, 3.4, 5.0):                # the whole of experiment 2
    p, _ = bench(v, 0.012, 1.6)
    print(f"v={v:>4}  {p*1000:6.1f} mm   e/v = {p/v*1000:.2f} mm per m/s")
# v= 0.5     4.8 mm   e/v = 9.52 mm per m/s
# v= 1.4    13.3 mm   e/v = 9.52 mm per m/s
# v= 2.2    20.9 mm   e/v = 9.52 mm per m/s
# v= 3.4    32.4 mm   e/v = 9.52 mm per m/s
# v= 5.0    47.6 mm   e/v = 9.52 mm per m/s

2. The shake-test offset estimator

Fix 2 of the ladder — "measure once, subtract a constant" — is the rung everyone reaches for first, and it is the one whose implementation nobody shows. Here is the whole thing. You shake the robot by hand for twenty seconds, take the gyro's yaw rate on one side and a camera-derived rotation rate on the other, and find the shift that lines them up.

Four steps, and every one of them is a place implementations go wrong: resample onto a common grid (two rates cannot be correlated sample-against-sample), zero-mean both (a gyro bias is DC, and DC correlated against DC swamps the peak), correlate across the lag range, then interpolate the peak to sub-sample resolution.

python
import numpy as np

def estimate_td(t_gyro, w_gyro, t_cam, w_cam, fs=400.0, max_lag=0.100):
    """td, in seconds, to SUBTRACT from the camera stamps to reach the IMU base.
    w_* are scalar rotation-rate magnitudes; the sign convention is fixed here,
    once, and asserted in CI -- see failure mode F7."""

    # --- 1. resample both streams onto ONE clock ------------------------------
    t0 = max(t_gyro[0],  t_cam[0])  + max_lag      # trim so every lag stays
    t1 = min(t_gyro[-1], t_cam[-1]) - max_lag      # inside both records
    grid = np.arange(t0, t1, 1/fs)
    a = np.interp(grid, t_gyro, w_gyro)              # 400 Hz -> grid (near no-op)
    b = np.interp(grid, t_cam,  w_cam)               # 30 Hz -> grid (upsampled)

    # --- 2. zero-mean, or DC buries the peak ----------------------------------
    a = a - a.mean()
    b = b - b.mean()
    na, nb = np.linalg.norm(a), np.linalg.norm(b)

    # --- 3. normalised cross-correlation, one lag at a time --------------------
    K    = int(round(max_lag*fs))                     # 100 ms at 400 Hz = 40 samples
    lags = np.arange(-K, K+1)
    r    = np.empty(len(lags))
    for j, k in enumerate(lags):                   # r[j] = dot( a(t), b(t+k) )
        if k >= 0:
            x, y = a[:len(a)-k], b[k:]
        else:
            x, y = a[-k:],       b[:len(b)+k]
        r[j] = float(x @ y) / (na*nb)               # normalise -> r is a rho, not a scale

    # --- 4. parabolic sub-sample peak -----------------------------------------
    j = int(np.argmax(r))
    if j == 0 or j == len(r)-1:                       # peak pinned at the edge means
        raise ValueError("peak at lag limit -- widen max_lag")   # max_lag is too small
    y0, y1, y2 = r[j-1], r[j], r[j+1]
    delta = 0.5*(y0 - y2) / (y0 - 2*y1 + y2)         # in samples; |delta| <= 0.5
    return (lags[j] + delta) / fs, float(r[j])

Drive it with a synthetic shake carrying a known 12.0 ms offset, exactly as the regression-test flow above demands:

python
TD_TRUE = 0.012

def shake(t):
    """A hand shake with three tones, so the correlation peak is sharp."""
    return (1.9*np.sin(2*np.pi*1.7*t)
          + 1.1*np.sin(2*np.pi*4.3*t + 0.7)
          + 0.6*np.sin(2*np.pi*9.1*t + 2.1))

t_gyro = np.arange(0.0, 20.0, 1/400.0);  w_gyro = shake(t_gyro)
t_cam  = np.arange(0.0, 20.0, 1/30.0);   w_cam  = shake(t_cam - TD_TRUE)
# the camera stamps are LATE: the frame labelled t actually saw the world at t - td

td_hat, rho = estimate_td(t_gyro, w_gyro, t_cam, w_cam)
print(f"td_hat = {td_hat*1000:.3f} ms   rho = {rho:.4f}")   # td_hat = 11.848 ms  rho = 0.9957
assert abs(td_hat - TD_TRUE) < 0.0005                 # the assertion from box 3, passing

And now the library one-liner, which you should reach for in production and be able to replace at a whiteboard:

python
from scipy.signal import correlate, correlation_lags

fs   = 400.0
grid = np.arange(0.1, 19.9, 1/fs)
a = np.interp(grid, t_gyro, w_gyro); a -= a.mean()
b = np.interp(grid, t_cam,  w_cam ); b -= b.mean()

r    = correlate(b, a, mode='full')                       # FFT-based, O(n log n)
lags = correlation_lags(len(b), len(a), mode='full') / fs
print(f"td_hat = {lags[np.argmax(r)]*1000:.3f} ms")          # td_hat = 12.500 ms

Failure mode 7: the correction with the wrong sign

Why the bench draws the error twelve times larger

The orange trace is the true error multiplied by twelve. That is a deliberate, labelled exaggeration, and it is worth understanding why it is necessary — because the reason is the reason this bug survives in production.

At 3.4 m/s with a 12 ms offset the peak cross-track error is 32.4 mm on a 96 m route with a 1.6 m slalom amplitude. Express that as a fraction of the picture:

A third of a pixel. Nobody has ever spotted this bug by looking at a trajectory plot, and nobody ever will. It is a five-centimetre error hiding inside a hundred-metre picture, and the only way to see it is to compute a residual and plot that — against speed, against turning, against uptime.

The same law, three real routes

Θ is a property of the route, not of the robot, so the same 12 ms produces very different damage depending on where the robot works. This table is worth carrying into a design review, because it turns "our sync is a bit off" into a deployment-specific number.

When the offset comes out negative

Drag the slider left of zero. The path still bends, mirrored. That is not an academic case — a negative measured offset (the camera appearing earlier than the IMU) is common, and it has three distinct causes that you diagnose differently.

The three numbers to memorise

If you retain nothing else from this chapter, retain these. Each is one multiplication and each answers a question you will be asked.

Scaling the bench to a fleet

One robot on a bench is a debugging tool. Sixty robots in a city needs the same physics turned into a monitored signal, and this is the part that separates a demo from a deployment.

The whole argument, rehearsed

Here is how this material actually gets used, as a compressed transcript of a design-review conversation. What matters is not the first answer — it is how many follow-ups the answer survives.

Seven exchanges, and every one of them is a number or a mechanism from this lesson. Notice what never appears: no filter tuning, no vague appeals to "more data", no claim that a learned model would help.

Sanity checks that catch a broken bench

If you build one of these yourself — and you should, it is an afternoon — these four checks catch almost every implementation bug before it teaches you something false.

What the bench deliberately leaves out

Someone will push on the model's limits, and knowing them is more valuable than defending the model.

The frontier

The five-fix ladder is not folk knowledge. Every rung of it is a published, citable method with a name attached. Being able to say "that is the Kalibr formulation" instead of "you measure it somehow" is the difference between using the field and reinventing it.

Rung 2 — and why Kalibr is more than the cross-correlation you just wrote. Mair et al. is essentially the estimator from the code section: excite the rig, correlate two rotation-rate signals, take the peak. It returns one scalar and nothing else. Furgale, Rehder & Siegwart do something structurally different — they solve for t_d jointly with the 6-DoF spatial extrinsics inside a single continuous-time batch estimator, representing the trajectory as a B-spline so that pose is differentiable at any instant rather than only at sample times, and they return a joint covariance over all of it.

That joint solve is not academic tidiness. On a turning trajectory, a mis-estimated t_d and a mis-estimated lever arm between camera and IMU produce nearly the same residual — both look like "the camera saw the feature from slightly the wrong place". Solve for one while holding the other fixed and the error simply relocates into whichever parameter you left free, and both come out confidently wrong with small-looking uncertainties. The one-line version to say out loud: "temporal and spatial calibration are correlated on rotating trajectories, so I would solve them together rather than in sequence."

Rung 4 — where the "< 0.01 ms" number comes from. With hardware timestamping in the PHY at both ends and transparent or boundary clocks in between, PTP holds PHC-to-PHC offset in the tens-to-hundreds-of-nanoseconds range: three to four orders of magnitude better than the 0.1–1 ms that NTP over a quiet LAN delivers, because NTP's stamps are taken in userspace and inherit every bit of scheduler jitter on the way.

The 10 µs in that ladder cell is therefore deliberately conservative, and knowing why is the detail that shows you have operated this rather than read about it: the nanoseconds live between the two hardware clocks, but almost all robot software reads CLOCK_REALTIME, which reaches the PHC only through phc2sys — a userspace daemon. That last hop is the microsecond-scale part of the chain, and it is also the part that dies silently, which is exactly failure mode F1 from Chapter 1.

Rung 5 — two different ways to make t_d a state. Li & Mourikis append t_d to the MSCKF state vector, so the filter estimates it alongside pose, velocity and IMU biases, and it tracks thermal and exposure drift for free — the two things that make the calibrated constant of rung 2 go stale. Qin & Shen make the same idea cheap enough to drop into an existing optimiser: rather than re-timing the whole measurement, they shift each feature observation in the image plane by t_d times that feature's measured pixel velocity, which turns the reprojection residual into an explicit, differentiable function of t_d. One extra scalar in the state, one extra term in the Jacobian.

Chapter 5: Field Guide

Sooner or later the ticket lands on you: "Our fleet localises fine in the depot and drifts at highway speed — and it ships in six weeks." Everything below is what has to be loaded before that sentence lands. A live incident is not the time to derive this material. It is only the time to use it.

Seven sections: the cheat sheet (with the three derivations worth being able to do at a whiteboard directly underneath it), system-design patterns, coding drills, debugging scenarios, three debugging dialogues written out in full, the classical-versus-modern call, and what to read next.

1 · The cheat sheet

1b · The three derivations worth being able to do at a whiteboard

A cheat sheet is a lookup table, and there is a real difference between reading one off the inside of your skull and rebuilding it. Twelve rows are up there; three of them come up constantly — in design reviews, in onboarding sessions, in postmortems — in the form "derive that for me, on the board, now." Those three are below with every algebraic step written out, in the order you should write them.

(i) The NTP offset, and the exact line where it dies

Set the notation up before writing a single equation, because this derivation is pure bookkeeping and the bookkeeping is where people fall over. There are two clocks. The single unknown is θ, the amount the server clock is ahead of the client clock. There are two transit delays, and the entire point of the exercise is that they are different numbers: d_out for client→server and d_back for server→client. And there are four observations, each read on the clock belonging to whoever wrote it down.

Step 1 — write each arrival as departure + delay + clock conversion. The request leaves at client-time t₁, spends d_out in flight, and is then read on a clock sitting θ ahead. So you add θ:

The reply leaves at server-time t₃, spends d_back in flight, and is read on a clock sitting θ behind. So you subtract θ:

Step 2 — rearrange each into a difference you can actually form. You may only subtract two stamps taken on the same clock — that constraint is what forces the pairing to be (t₂−t₁) and (t₃−t₄) rather than the more natural-looking (t₂−t₁) and (t₄−t₃). Say that sentence out loud when you write this line; it is the step everyone fumbles.

Step 3 — add them. The two θ terms reinforce, and the two unknown delays subtract:

Divide by two and solve for the thing you want:

Step 4 — the line that dies. Look at what is now on the right. The first bracket is built entirely from the four stamps, so it is measurable. The second term contains d_out and d_back separately, and no combination of the four stamps can ever separate them — every observable involves only their sum. So you delete it, and the deletion is not algebra, it is an assumption:

Step 5 — subtract instead of adding, and the bound falls out. Form the round trip from the same four stamps, this time pairing them so that θ cancels:

Both delays are physically non-negative, so each is somewhere in [0, RTT], which means |d_out − d_back| ≤ RTT and therefore:

That is the whole reason a round trip is worth measuring. It does not improve the estimate by one nanosecond — it brackets it. This is exactly Spanner's TrueTime move, made eleven years before Spanner.

(ii) The linear-interpolation error bound, and where the 8 comes from

The cheat sheet asserts |x″|·h²/8. Nearly everyone can quote it; almost nobody can say why the denominator is 8 rather than 2 or 4, and that is precisely the follow-up. It comes from two places multiplied together, and you should derive them separately.

Step 1 — set up the interpolant. Two samples, x(t_i) and x(t_i+1), a spacing h = t_i+1 − t_i, and a query somewhere between them written as t = t_i + u·h with u in [0, 1]. The linear interpolant is:

Step 2 — construct the error function, and use the trick. Fix the query point t. Define a helper function of a free variable s:

and choose the constant K so that g(t) = 0. That is always possible, because the quadratic factor is non-zero strictly between the samples. Now count the roots of g: it vanishes at s = t_i (both x and L pass through the sample, and the quadratic factor is zero), at s = t_i+1 for the same reason, and at s = t by construction. Three roots.

Step 3 — apply Rolle twice. Three roots of g means two roots of g′ (one between each adjacent pair), and two roots of g′ means one root of g″. Call it ξ, and it lies somewhere inside the interval. Differentiate the definition of g twice: L is linear so L″ = 0, and the quadratic (s−t_i)(s−t_i+1) = s² − (…)s + (…) has second derivative exactly 2. So:

Substituting back into g(t) = 0 gives the exact error at the query point, with no approximation anywhere:

Step 4 — convert the product to u, and maximise it. With t = t_i + uh we have (t − t_i) = uh and (t − t_i+1) = (u − 1)h, so their product is u(u−1)h² = −u(1−u)h². Take absolute values:

Maximise u(1−u) over [0, 1]. Differentiate: 1 − 2u = 0, so u = ½, and the value there is ½ × ½ = ¼. That is the second factor. Multiply the two:

The zero-order-hold bound is the same argument truncated one term earlier. Hold the last sample and the error is |x(t) − x(t_hold)| ≤ |x′| · |t − t_hold|; nearest-neighbour selection guarantees the hold distance never exceeds h/2, giving v · h/2. First order in h, and it scales with velocity. The linear bound is second order in h, and it scales with acceleration. Those are different physical quantities, which is why the ratio is not a constant.

(iii) ∂r/∂t_d = −J · v, and why the robot has to move

This one gets asked as "you said you'd put the time offset in the state — what's its Jacobian?", and the correct answer is short, so the risk is answering too fast and missing the observability half.

Step 1 — write the residual with t_d visible in it. A feature is measured in the image at pixel u_meas. The image is labelled with time t, but its true capture instant is t + t_d. So the prediction must evaluate the geometry at t + t_d, not at t:

where p_c is the landmark expressed in the camera frame — a 3-vector in metres — and π is the projection, metres to pixels. Everything with a time argument is a trajectory, which is why this is a natural fit for the continuous-time representation of Chapter 3.

Step 2 — differentiate, one chain-rule link at a time. Only the second term depends on t_d, and it depends on it only through p_c:

Step 3 — name both factors. The first is J, the 2×3 projection Jacobian, in pixels per metre. It is not a new object — it is the identical matrix the bundle adjustment already builds for every observation, which is why adding t_d to the state is nearly free. The second is the landmark's velocity in the camera frame; for a static landmark it is the negative of the camera's own velocity, expressed in the camera frame. Call it v, in metres per second. So:

Step 4 — check the units, out loud. J is px/m, v is m/s, so the product is px/s. It is a 2-vector of pixels per second. That is not a coincidence and it is not an analogy — the Jacobian of the residual with respect to the time offset is the feature's image velocity. Multiply it by a t_d in seconds and you get pixels, which is what a residual is measured in. Saying this sentence is worth more than writing the equation.

Step 5 — the observability half, which is the actual point. Look at what happens when the camera stops. v = 0, so ∂r/∂t_d = 0 identically: the entire column of the Jacobian is zeros. In the normal equations H^TH that produces a zero eigenvalue in the t_d direction, the system is singular, and the covariance of t_d grows without bound. There is nothing subtle about it — the residual does not change when t_d changes, so no amount of data can tell you what t_d is.

2 · System-design patterns

Prompt A — "Design the timing architecture for a six-sensor autonomous vehicle rig."

Before the framework, put the rig on the board. A design answer that names sensor classes is boxes and arrows; a design answer that names rates, payload sizes, transports and measured latencies is an engineering position. Draw this table first — it takes ninety seconds and it makes every later sentence checkable.

Prompt B — "Design bag recording and replay so that timing bugs reproduce."

Prompt C — "A vendor LiDAR only exposes receive timestamps. What do you do?"

Prompt D — "The robot must run eight-hour shifts with no recalibration."

3 · Coding drills

Drill 1 — "Implement a time-aligned pose lookup." The most commonly asked of the four.

Declare the shapes before you write the body — write the comment block first. Pinning down units and component order before touching logic is what keeps the body to five honest lines.

python
# ts   : (N,)  float64 — seconds, CAPTURE times, strictly increasing
# pos  : (N,3) float64 — metres, in the map frame
# quat : (N,4) float64 — UNIT quaternions, component order (w, x, y, z)
#                          map_R_body, i.e. body-to-map. Eigen and scipy
#                          both hand you (x, y, z, w) — convert at the
#                          boundary or the rotation is silently wrong.
# tq   : ()    float64 — seconds, the instant to evaluate
# returns ((3,) float64 metres, (4,) float64 unit quat (w,x,y,z)) or None
# slerp() is the Chapter 3 implementation: same (w,x,y,z) order, short-arc
# flip on negative dot, NLERP fallback under 1.81 deg of separation.
def pose_at(ts, pos, quat, tq):
    if len(ts) < 2 or tq < ts[0] or tq > ts[-1]:
        return None                                  # refuse; never clamp
    i = min(np.searchsorted(ts, tq, 'right') - 1, len(ts) - 2)
    u = (tq - ts[i]) / (ts[i+1] - ts[i])
    return pos[i] + u*(pos[i+1] - pos[i]), slerp(quat[i], quat[i+1], u)

Drill 2 — "Estimate the time offset between two rate signals."

python
# a, b : (N,) float64 — rad/s, the SCALAR gyro magnitude |omega| of each
#        stream, already resampled onto ONE common uniform grid at fs.
#        Magnitude, not the 3-vector, so no frame alignment is needed —
#        |omega| is rotation-invariant, which is the whole trick.
#        b is the stream suspected to be LATE.
# fs      : () float  — Hz, the common grid rate (200.0 here)
# max_lag : int, SAMPLES of search either side of zero
# guard   : int, SAMPLES trimmed from both ends so every lag scores the
#           same window length. Requires guard >= max_lag.
# returns () float — SECONDS. Positive means b lags a.
def estimate_td(a, b, fs, max_lag=40, guard=50):
    a = a - a.mean(); b = b - b.mean()          # DC would dominate the score
    N = len(a); lags = np.arange(-max_lag, max_lag+1)
    c = np.array([a[guard:N-guard] @ b[guard+l:N-guard+l] for l in lags])
    k = int(c.argmax())
    y0, y1, y2 = c[k-1], c[k], c[k+1]
    d = 0.5*(y0 - y2) / (y0 - 2*y1 + y2)          # parabolic vertex
    return (lags[k] + d) / fs                        # seconds

Run it, by hand, on real numbers. Saying "I'd cross-correlate" is a claim. Walking through one call — inputs, intermediate values, output — is a demonstration, and it takes ninety seconds.

python
# Build two 200 Hz traces offset by EXACTLY 12 ms. 12 ms is 2.4 samples
# at 200 Hz — deliberately non-integer, so the sub-sample step has work.
>>> fs = 200.0; t = np.arange(2000) / fs          # (2000,) float64, 10.0 s
>>> sig = lambda u: np.sin(2*np.pi*1.7*u) + 0.6*np.sin(2*np.pi*4.1*u)
>>> a = sig(t)                                  # (2000,) rad/s
>>> b = sig(t - 0.012)                          # (2000,) rad/s, 12 ms LATE
>>> estimate_td(a, b, fs)
0.012009

Now degrade the input and look at what actually comes out. This is the half of the drill that separates people who have run this on a robot from people who have read about it. The failure is not an exception, and it is not a None. It is a confident, precise, catastrophically wrong float.

python
# Same rig, same 12 ms offset — but the robot is creeping down a
# corridor. |omega| is a slow drift plus noise, not an excitation.
>>> rng = np.random.default_rng(7)
>>> drift = lambda u: 0.30 + 0.02*np.sin(2*np.pi*0.05*u)
>>> a2 = drift(t) + rng.normal(0, 0.004, t.shape)
>>> b2 = drift(t - 0.012) + rng.normal(0, 0.004, t.shape)
>>> estimate_td(a2, b2, fs)
-0.149340                       # −149.34 ms. Truth is +12.00 ms.
>>> c.min(), c.max()          # excited run: (-368.55, 1295.56)
(0.060268, 0.063313)         # 81 lags, total swing 4.8%

Drill 3 — "Back-date a FIFO burst." Four lines, and the sign is the whole exercise.

python
# t_rx        : () float64 — SECONDS, host monotonic time the BATCH arrived
# n           : int        — samples in this batch (32 for our IMU)
# fs          : () float   — Hz, nominal sample rate (400.0)
# transport_s : () float   — SECONDS, MEASURED mean transport + driver
#               latency. Mean only — the jitter around it is a separate
#               budget line and cannot be back-dated away.
# returns (n,) float64 — SECONDS, capture times, OLDEST FIRST, matching
#         the sample order inside the packet.
def capture_times(t_rx, n, fs, transport_s):
    """A burst ARRIVES together; it was not CAPTURED together."""
    newest = t_rx - transport_s
    return newest - np.arange(n-1, -1, -1) / fs   # oldest first

Drill 4 — "Write a fixed-lag synchroniser." The one that separates people who have shipped this.

python
# names : list[str]  — one per stream, e.g. ['imu', 'wheel', 'lidar']
# depth : int        — samples retained per stream. Set it from
#         max_query_lag * rate: 300 ms * 400 Hz = 120, round up to 256.
# push(name: str, t: float64 SECONDS capture time, monotone per stream,
#      v: Any — a (3,) float64 ndarray, a (4,) quaternion, an image
#      handle. The class never looks inside v; interpolation is the
#      caller's job, which is why it works for rotations too.)
# query(tq: float64 SECONDS) -> None   registers a wanted instant
# drain() -> list[float64] SECONDS, ASCENDING, of released query times
# Memory: 3 streams * 256 * (8 B time + 24 B vec) = 24.6 kB, forever.
class FixedLagSync:
    """Emit (tq, values...) only when EVERY stream brackets tq."""
    def __init__(self, names, depth=256):
        self.buf = {n: [] for n in names}; self.depth = depth
        self.pending = []                       # query times awaiting release

    def push(self, name, t, v):
        b = self.buf[name]; b.append((t, v))
        if len(b) > self.depth: b.pop(0)       # drop oldest, bounded memory

    def query(self, tq): self.pending.append(tq)

    def drain(self):
        out = []
        for tq in sorted(self.pending):
            if any(not b or b[-1][0] < tq for b in self.buf.values()):
                break                           # still waiting: DO NOT extrapolate
            if any(b[0][0] > tq for b in self.buf.values()):
                self.pending.remove(tq); continue   # too old: drop and COUNT it
            out.append(tq); self.pending.remove(tq)
        return out

4 · Debugging scenarios

5 · Three debugging dialogues

Everything above is material. This is practice, and they are not the same skill. Below are three debugging conversations on this topic, written out as dialogue: a mentor's prompt, your first answer — which is deliberately the shallow or wrong one, because that is what actually comes out of your mouth under time pressure — the probe that follows, and the corrected answer.

Three dialogues, drawn from three of the six symptoms in the table above. Read the mentor's lines out loud. Cover your own lines and answer before you look.

Dialogue 1 — "It gets worse with speed."

Dialogue 2 — "It explodes twice a shift and replays clean."

Dialogue 3 — "The arm misses when the base is moving."

Coda — two questions worth asking any robotics team

6 · Classical vs modern

7 · Recommended reading

The one book. Timothy Barfoot, State Estimation for Robotics (Cambridge, 2nd ed. 2024). It is the only mainstream robotics text that treats continuous-time estimation as a first-class citizen rather than a footnote, which is exactly the deep version of Chapter 3. Chapters 3 and 4 give you the Gaussian-process trajectory formulation; the Lie-group chapters give you why SLERP is the right thing and what its generalisation looks like. Free PDF from the author's page. If you only read fifty pages, read the continuous-time section.

The budget calculator

Where this lesson connects