Capture & Composition

Stage benches localize a fault. Two things were missing: real data inside the benches, and evidence that a fix localized in a bench also fixes the system. Capture supplies the first; the loop checks supply the second.

Real runs become fixtures (#446)

Any moment of a run can be replayed in its stage’s bench and debugged at unit-test speed. The backend reports its stage boundaries to an optional tap; a recorder turns each one it selects into a captured fixture, holding the stage’s input and the output the run produced.

MsckfBackend ──set_stage_tap(&recorder)──▶ StageRecorder<T> ──▶ DIR/<stage>/<name>.json
 (sdk: stage_tap.hpp)                       (tools: stage_recorder.hpp)        │
                                                                               ▼
                                              s6d_gating_bench --fixture f000013_S6d_gating_feat127.json

From the command line

vio_pipeline captures from the synthetic world or from EuRoC:

# every boundary of post-init frame 13 (the frame numbering starts at initialization)
./build/tools/vio_pipeline --capture cap/ --capture-frames 13:13

# EuRoC: only the S6 updates whose NIS/dof exceeds 5, every one through S5 → S6e
./build/tools/vio_pipeline --source euroc --dataset .../MH_05_difficult/mav0 \
    --capture cap/ --capture-nis 5

# the first boundary, anywhere in the run, whose fixtures fail their bench
./build/tools/vio_pipeline --capture cap/ --capture-first-violation

# then replay one in its bench: every type, every variant, in seconds
./build/tools/s5_triangulation_bench --fixture cap/S5_triangulation/f000002_S5_triangulation_feat19.json

--capture-stages S2_propagation,S6d_gating narrows the stages and --capture-max N caps the count. In C++, set a StageRecorder<T> on any MsckfBackend<T> with set_stage_tap.

What is captured

Backend boundary	Fixtures (one bench each)	Expected output
S0, each frame and camera	`S0_sensor_model`: its pixels and intrinsics	the run’s bearings and validity
S2, each IMU step and the hold to the frame time	`S2_propagation`: state, sample, noise, gravity	the run’s state after
S3	`S3_augmentation`: state, clone time	the run’s state after
S6, each feature track	`S5_triangulation`, `S6a_jacobians`, `S6b_nullspace_projection`, `S6c_compression`, `S6d_gating`, `S6e_ekf_update`: as far as the track got	the run’s gate decision, NIS and covariance after; the intermediates recomputed
S9	`S9_marginalization`: state, clone index	the run’s state after

The transformations themselves are not stored as matrices. Each bench rebuilds them from the captured input, bit for bit: F/Φ and Q_d (S2), the clone Jacobian (S3), H_x, H_f and r (S6a), the projected H₀ and r₀ (S6b), S (S6d), K and δx (S6e). The intermediates the run hands from one S6 sub-step to the next (p_f, the measurement system, the projected system) are captured as the next sub-step’s input. The state before and after every stage is in the fixture: nav state, biases, clones, calibration and P.

The S6 intermediates are not exposed by the backend, so the recorder recomputes the chain with the same stage functions. It then checks the chain against the run: the outcome, the NIS and the state after must equal the run’s bit for bit. A difference is counted and reported, never hidden.

Each fixture is tagged with the arithmetic the run used and the platform it was captured on, so replay.bit_identical holds there. S1 is captured per dynamic-init attempt, before the filter starts: the SfM window, the static IMU window, and the bench’s sfm_two_view_pnp run as the expected output, checked against what the backend did. Because those attempts come before frame 0, they are captured only when the frame range starts at frame 0 (the default) and no trigger (--capture-nis, --capture-first-violation, --live-assert) is set; otherwise no attempt produces an S1 fixture. To capture moving-start windows from EuRoC:

./build/tools/vio_pipeline --source euroc --dataset …/MH_05_difficult/mav0 --prefer-dynamic \
    --capture cap/ --capture-stages S1_initialization

Not captured: S4 (the frontend: the backend never sees images; asl_trace covers S4) and S10. A multi-camera updater’s S6 boundaries are skipped, because the S5 and S6a benches model one camera.

What selects a capture

The stage: any of the fixture stages above.
A frame range: post-initialization frames, inclusive. S2 steps belong to the frame they propagate to.
A trigger:
- NIS: an S6 update whose NIS per degree of freedom exceeds a threshold;
- first violation: the first boundary whose fixtures fail their bench (invariants and replay).
With a trigger set, a boundary is captured only when a trigger fires on it.

Cost when off

The tap is a pointer; off, each stage boundary costs one null test, and nothing is copied. A tap that wants a boundary is the only thing that copies the state. Measured on the synthetic world (250 frames, Release):

Run	Wall time (median of 7)	Outputs
`main`, before the tap existed	1.2690 s	—
this branch, capture off	1.2692 s	`trajectory.csv`, `run.jsonl` and stdout byte-identical to `main`

With capture on (single runs, same world):

Selection	Wall time	Captured
`--capture-frames 13:13` (every stage, one frame)	1.28 s	49 fixtures, 19 boundaries, 11 MB
`--capture-nis 5`	2.15 s	215 fixtures, 43 S6 updates, 31 MB
`--capture-first-violation`	1.28 s	1 fixture (frame 2, S5)
`--capture-stages S9_marginalization`	1.54 s	239 fixtures, 82 MB

A test (stage_capture) also checks that a tap which captures nothing, or every stage, leaves the filter’s final state and NIS bit-identical.

What capture found

Run on the synthetic world with --capture-first-violation, the first frames after initialization turned up two things the stage benches’ own fixtures had not:

S5 rejects tracks whose rays diverge, and its contract calls that a failure. A two-view track at the image edge (0.74° of parallax, normal-matrix κ ≈ 2.4e4) with ordinary pixel noise has rays that meet 6 cm behind the cameras. Every method rejects it: the shipped solve, the midpoint, the DLT and inverse depth. The bench’s triangulation.resolved exempted only a parallax gate, so a correct rejection read as a violation. It was the run’s first violation, within three frames of initialization. Since #487 the contract also exempts a rejection when no positive-depth solution exists: the least-squares point of the rays is behind a camera, or the rays are parallel. It reports that point’s depth as triangulation.linear_min_depth. On the synthetic world, up to 3 px of noise, S5 then has no violation in 120 frames.
The S6d symmetry check was too tight. The bench rebuilds S = H·P·Hᵀ + σ²I and held its asymmetry to the scale of S (σ² ≈ 1e-4). The products in H·P·Hᵀ cancel, so their rounding scales with |H|·|P|·|H|ᵀ instead; a real update showed 3.8e-18. The bench now scales the tolerance by the products’ magnitude.

S0 capture once ran into the synthetic camera’s p₂, which was 1000× EuRoC’s until #467 fixed it. The recorder takes the backend’s camera through intrinsics_of, never a copy of EuRoC’s values, so a capture always records the camera the run actually used.

The loop is checked as a loop (#447)

Some failures only emerge from stages running together, and over-confidence is the main example. The stage benches localize a fault; this layer confirms that a fix fixes the system. It has three parts: the composition bench, live assertion, and a trajectory-level report.

The composition bench

c_filter_loop_bench runs the loop itself: a tape of stage operations in the order the backend ran them, from a starting state, through the shipped stage functions.

S2 … S2 → [S6 per track on the oldest clone → S9] → S3 → S6 per ended track → S2 …

At every operation, that operation’s own stage contract is checked: the stage bench’s invariants, evaluated on the loop’s input and output for it. A fault is therefore attributed to the stage that introduced it. The report names each result <stage>.<invariant>, puts the first violation first, and gives its operation index (loop.first_violating_op).

Loop-level results cover what no single stage owns:

Result	What it is
`loop.covariance_symmetric`, `loop.covariance_psd`	the covariance the loop ends with
`loop.unobservable_leak_max`, `_mean`	per applied update, the share of the information it adds along the four unobservable directions, ‖H₀N̂‖²/‖H₀‖²
`loop.nis_per_dof`	over the loop’s updates
`truth.nav_nees`	ground truth: the final nav state’s NEES (15 dof), bound χ²₀.₉₉₉ = 37.7

Its fixtures are the three kinds every bench has:

Fixture	Input	Judged against
`known_answer_constant_velocity`	constant velocity, exact observations, P₀ = 0, Q = 0: 68 operations through S2, S3, S5–S6e and S9	the mean follows the motion exactly, P stays 0, every NIS is 0
`ground_truth_synthetic_three_frames`	a synthetic run’s tape (post-init frames 16–18), its starting mean reset to truth	NEES of the final nav state
`captured_synthetic_three_frames`	the same tape from the run’s own state	the run’s final state and per-update NIS, bit for bit

All three pass in double, float, posit<32,2> and long double. The sweep runs 1 to 16 cycles.

Injected faults are caught where they are injected. Two variants inject a defect into one stage:

fault_s3_clone_jacobian perturbs the new clone’s cross-covariance. The first violation is S3_augmentation.augmentation.blocks, at the first S3 operation.
fault_s6a_jacobian scales a clone-rotation block of H_x by 1.5. The first violation is S6a_jacobians.jacobian.state_vs_fd, the finite-difference check, at the first S6 operation.

Capture a loop from any run. LoopTape<T> is a stage tap (#446) that records the tape:

./build/tools/vio_pipeline --source euroc --dataset .../V1_01_easy/mav0 --capture cap/ --capture-loop 100:102
./build/tools/c_filter_loop_bench --fixture cap/C_filter_loop/loop.json

A V1_01 loop over frames 100–102 (38 operations) replays bit for bit and satisfies every stage and loop invariant.

Live assertion

vio_pipeline --live-assert checks every stage boundary of a full run against its bench, as it happens. It reports the first violation: stage, frame, invariant, value and bound. With --capture it also writes that boundary as a fixture.

live assertion  : FIRST VIOLATION S5_triangulation, frame 114 (t=1.40372e+09 s), reprojection.max_residual = 0.0516872 (bound 0.05)

It is the stage recorder (#446) with the first-violation trigger, so nothing new runs on the filter’s path. A normal build never sets a tap, and its outputs are byte-identical to main. On V1_01 the first violation used to be at frame 4: a two-view track whose rays diverge, correctly rejected and still counted as a failure (#487). With that fixed, the run gets to frame 114. There a track reaches S5 whose best point leaves an observation 5.2σ from its measurement: an outlier the front end passed on.

A test checks the attribution on a running filter. A fault injected into S3 at post-init frame 0 is reported as S3_augmentation, frame 0.

Trajectory-level consistency on EuRoC

vio_trajectory_report --dataset <mav0> --label NAME --out DIR runs the MSCKF over a sequence and writes NAME_report.json and per-window NAME_windows.csv. It reports:

NEES, gauge-anchored at the first post-init frame, overall and per block;
NIS over all updates;
the update leak, using the stage tap to recompute each applied update’s H₀;
position-σ growth, and how often the position error exceeds 3σ;
ATE, and RPE over 1 s segments.

	V1_01_easy	MH_05_difficult	V2_03_difficult
frames (init)	2907 (static)	2267 (static)	1889 (static)
ATE	0.289 m	0.755 m	0.269 m
RPE, 1 s	0.093 m	0.246 m	0.117 m
NEES / dof	436	42.8	140
NEES per block: attitude, position, velocity	993, 3.96, 19.9	73.1, 55.2, 47.5	214, 9.71, 63.6
NIS / dof	1.51	2.16	14.7
update leak ‖H₀N̂‖²/‖H₀‖²: max	9.0e-32	6.5e-32	1.0e-31
updates with a leak above 1e-6	0 of 35 686	0 of 27 915	0 of 16 265
position σ, start → end	0.18 → 0.26 m	0.29 → 0.42 m	0.17 → 0.25 m
position error beyond 3σ	15 % of frames	81 %	50 %

What it says. The filter is over-confident on all three sequences, worst in attitude on V1_01. That is the #212 finding, now per sequence and per block. Across all 79 866 updates on three sequences, no camera update adds information along the unobservable directions: the largest share is 1e-31, rounding. That confirms #480 on real data: no single update is where the filter learns what it cannot observe. The composition bench reports S2’s observability.yaw_leak (Φ against the yaw direction) at up to about 2e-6 per IMU step, on synthetic and EuRoC loops alike. The ledger below shows that this is a small part of the leak. Most of it comes from updates moving the linearization point.

The observability ledger: which stage leaks

A per-update audit checks H·N(x̂) at the state the update sees. A filter can pass it on every update and still gain information about yaw. Every Jacobian annihilates the basis at its own linearization point, but the stages don’t share one linearization point. vio_pipeline --observability-ledger (tools/include/branes/tools/observability_ledger.hpp, #480) measures what that costs. It carries the 4-D unobservable basis through the run the way the filter’s linear system carries it:

S2 multiplies it by the propagator’s own Φ.
S3 copies the IMU pose rows into the new clone.
S9 drops the clone’s rows.
An S6 update leaves it alone.

In that system, the camera’s Fisher information about the starting unobservable directions is Σ Tᵀ Hᵀ R⁻¹ H T over applied updates, and a consistent filter keeps it at 0.

The carried basis T drifts away from N(x̂) only through each stage’s defect, carried forward by the later stages:

S2: Φ N(x̂⁻) − N(x̂⁺).
S3: the clone copy against the new basis, exactly zero.
S6: N(x̂⁻) − N(x̂⁺), the update moving the mean.
S9: zero.
Untapped: any change no tap reported.

So R^-½ H T splits exactly by stage, and the shares add up to the total. A self-check holds T = N(x̂) + Σ D to roundoff.

	V1_01	MH_05	V2_03
S6 applied updates	34 788	27 513	14 799
closure, T against N(x̂) + Σ D	1.2e-10	4.3e-11	1.2e-10
share of the spurious yaw information: S6 moving the mean	99.4%	99.1%	94.7%
… S2, Φ against the mean’s Euler step	0.6%	0.9%	5.3%
… S3, S9, S6’s own projection, untapped	0	0	0
information along global translation	1e-22	7e-23	5e-23
filter yaw σ, start → minimum	0.1 → 0.027 rad	0.1 → 0.024 rad	0.1 → 0.019 rad

The ledger ignores process noise, which only forgets information, so its totals are upper bounds (3.4e9 rad⁻² on V1_01). The shares are the finding. Translation never leaks, because its basis doesn’t depend on the state. Yaw leaks through the update: an update corrects the mean, the next Jacobians are linearized at the corrected mean, and the basis they annihilate has moved from the one the earlier Jacobians carried. S2’s share grows with rotation rate and stays a minor term. It comes from linearizing Φ against the mean’s Euler step, not from truncating Φ: the S2 bench finds a higher-order Φ leaks more. This is the mechanism that first-estimates Jacobians and the R-IEKF each target. The run is unchanged by the tap: ATE is bit-identical to an untapped run. With --out, the ledger also writes observability_ledger.csv, the running totals and per-stage yaw shares at every frame.

What the composition bench found

The S6d nis.matches_direct tolerance was too tight, for the same reason as the symmetry check in #446. The filter forms its own S = H·P·Hᵀ + σ²I, and the two S’s differ by rounding that scales with |H|·|P|·|H|ᵀ, not |S|. A captured loop showed 3.6e-15 against a 2.2e-15 bound. The tolerance now scales with the products.
#487 showed up in the loop too. The built-in tapes use post-init frames 16–18, because frames 13–15 and 20–22 hold a diverging-ray track. Since #487 a loop over frames 13–15 passes every invariant.

The resulting workflow

EuRoC run ──vio_pipeline --live-assert --capture cap/──▶ first violation: <stage>, frame F, <invariant>
     │                                                    + cap/<stage>/f<F>_<stage>_….json
     ├──<stage>_bench --fixture cap/<stage>/….json        (seconds, every type, every variant)
     │
     ├──vio_pipeline --capture cap/ --capture-loop F-2:F──▶ cap/C_filter_loop/loop.json
     │    c_filter_loop_bench --fixture …/loop.json        (the loop around it, every stage checked)
     │
     └──fix the stage in the filter (sdk/), re-run its bench and the composition bench, then
        confirm on the whole run: vio_trajectory_report (NEES, NIS, update leak, ATE/RPE)