Skip to content
Cortex

Divide-and-Conquer VIO Debugging

This section describes how cortex debugs, tests and characterizes its VIO pipeline: one transformation at a time, each in its own test bench, each held to explicit invariants, in several arithmetic types. It is the methodology of epic #444. It builds on the stage contracts of the VIO Pipeline section and the diagnostic methodology behind them.

Why: the end-to-end number can’t say where

Section titled “Why: the end-to-end number can’t say where”

The VIO filter still doesn’t produce a trustworthy estimate, and the end-to-end metrics can’t say where it goes wrong:

  • The filter is over-confident. NEES and NIS sit far above their χ² targets (#212). The leading suspects are properties of the stages running together, so watching the final trajectory doesn’t separate them.

  • A fix proven at one level underperforms at another. The right-invariant EKF removes the yaw leak at the Jacobian level (0.39 → 2e-16), yet runs worse than the body-frame MSCKF end to end.

  • Several failures were numerical, not algorithmic:

    • the square-root covariance diverged on real data (ATE 6e+40 m);
    • a barely-conditioned update produced a non-finite δx;
    • the invariant backend diverges under Clang but not GCC (#437: 58 m vs 3.4 m RMS).

    With only an end-to-end metric, an algorithmic fault and a precision or conditioning fault look the same.

ATE and RPE are measured after aligning away the very degrees of freedom VIO bugs live in, and a EuRoC replay takes minutes to return roughly one bit of information. The diagnostic methodology page explains why steering by that number fails. Epic #444 replaces it as the debugging instrument.

What: an instrumented, divide-and-conquer environment

Section titled “What: an instrumented, divide-and-conquer environment”

Every stage is a transformation

Each stage S0–S10 is an explicit function, (state, inputs, params) → (state, diagnostics). The backend is just their call sequence, so a stage can be run, tested and replaced on its own. #452

Every transformation has invariants

SO(3) membership, PSD, Loewner order, null-space annihilation, principal submatrix, χ² consistency… Each is a measured residual in native units, against a threshold that scales with the arithmetic type. #445

Every transformation has a bench

A small executable that runs one stage on fixtures, in several number systems and implementation variants, and prints its invariant report in seconds. The framework exists, and every implemented stage, S0–S6, S9 and S10, has one. #453

Real runs become fixtures

Any moment of a run, synthetic or EuRoC, is capturable at a stage boundary (by stage, frame range, NIS spike or first invariant violation) and replays bit for bit in that stage’s bench. #446

The loop is checked as a loop

The filter loop is benched as a tape of stage operations, every boundary is checked live in a full run, and a trajectory report measures NEES, NIS and the update leak against EuRoC ground truth, because over-confidence is a property of the cycle. #447

Mixed precision is a diagnostic

Planned. The pipeline’s linear algebra moves onto MTL5, generic in the arithmetic type. A stage whose output moves between double, posit<32,2> and float is numerically fragile. The bench framework already runs a stage in several types, today for every bench except S4 (the front end is not yet generic). The S6 benches found that a square-root filter must carry its factor: re-factoring the covariance in float or posit32 drops the clone directions. #448–#451

How: the method, step by step

Section titled “How: the method, step by step”

  1. Cut the pipeline at its stage boundaries. Each stage becomes a function whose only effect is on the state passed in, and which returns what happened instead of logging it. The backend becomes the readable call sequence S2 → S6 + S9 → S3 → S0 + S4 → S5 → S6a…S6e.

  2. Write down what each transformation must preserve. The invariant table gives every stage its checks, from timestamp monotonicity at the inputs to “δx has no component along the unobservable directions” at S6e. A wrong output is caught at the transformation that produced it, not three stages later.

  3. Give each transformation a bench. A bench runs that one stage on fixtures:

    • known-answer: synthetic input with an analytic expected output;
    • ground-truth-injected: truth fed in upstream;
    • captured: a stage boundary recorded from a real run.

    It runs each fixture in every arithmetic type and every implementation variant, and sweeps the conditions (noise, Δt, parallax, conditioning, window size) to CSV.

  4. Turn interesting moments of real runs into fixtures. When a EuRoC run misbehaves, capture the stage boundary where an invariant first fails and debug it in that stage’s bench, at the speed of a unit test.

  5. Confirm at the system level. Some faults exist only in the loop. Composition benches and trajectory-level NEES/NIS confirm that a fix localized by a bench also fixes the filter.

  6. Use the arithmetic as an instrument. Port the linear algebra to MTL5, stage by stage behind each bench, then run every stage in several number systems. Precision sensitivity becomes a per-stage measurement, not a guess.

Where it stands

Section titled “Where it stands”
StepIssueWhatStatus
1#452Stage interface: explicit transformations, the backend as their call sequencedone: bit-identical on EuRoC
2#453Bench framework: three fixture kinds, invariant reports, sweeps, multi-type, variantsdone: PR #463, with the S9 bench as worked example
3#445Invariant library: the benches’ oracle, with type-scaled tolerancesdone
4#454Stage benches: S0–S1 (sensor models, initialization)done: PR #466
4#455Stage benches: S2–S3 (propagation, augmentation)done: PR #470
4#456Stage benches: S4–S5 (frontend, triangulation)done: PR #473
4#457Stage benches: S6a–S6e (the MSCKF update)done: PR #479
4#458Stage benches: S9 policies, S10 (online calibration)done: PR #484
—#471, #472Follow-ups: S2 invariant-propagator and higher-order Φ variants; S1 SfM front-end variantsplanned
—#474, #574Found by the S4 bench: KLT border churn. The detection margin that removes it is opt-in, off by default for its EuRoC ATE cost; #574 finds why and flips the defaultpartial
—#475Found by the S4–S5 benches: the synthetic world’s velocity stepfixed
—#480Found by the S6 benches: attribute the #212 leak per stage. The observability ledger books 95–99% of it to S6 moving the linearization point, 1–5% to S2’s Φdone
—#482Found by the S6 benches: square-root variants must carry their factoropen
—#481Found by the S6d bench: the loose low-dof χ² gate, now the χ²₀.₉₅ quantilefixed
5#446Stream capture: real runs become bench fixturesdone: PR #486
6#447Composition benches, trajectory-level consistency, live-assertion modein review
—#487Found by capture: S5 counts a correct triangulation rejection as a violationopen
7–9#448, #449, #450MTL5 port per stage; genericity over non-IEEE types; per-stage precision characterizationplanned
10#451MTL5 gap register, gaps filed upstreamplanned
—#461sqrt-covariance update overflow, found by the methodopen

The epic subsumes two older diagnostics issues: per-component invariant self-checks (#263) and conditioning monitors (#265).

Reading this section

Section titled “Reading this section”