The Canonical VIO Pipeline

Purpose. This document goes back to first principles: it distills the canonical VIO pipeline as designed by OpenVINS (and contrasts MINS / VINS-Mono / ROVIO / ORB-SLAM3), decomposed into discrete stages. Each stage is written as a contract: a type signature, the pre-conditions that must hold on its inputs, the post-conditions / invariants that must hold on its outputs, the probes that measure whether those conditions hold, and the unit tests + sensitivity analyses that validate them. The code cites its sections (§S0– §S10); the stage pages explain each stage, and Engineering Status has what the probes found.

The method this enables: for every stage, build the probe, write the test, confirm the contract holds in isolation before composing. A VIO filter is a chain of transformations where one violated invariant three stages upstream surfaces as “drift” or “over-confidence” with no local symptom. Contracts localize the fault.

Companion: docs/arch/visual-inertial-odometry.md, the deep offline math treatment (sensor models, preintegration covariance, marginalization+FEJ, the structured solve, the frontend). This document is the operational counterpart: what the canonical reference systems actually do, stage by stage, and how to instrument each stage.

0. How to read this document

A VIO estimator is a typed dataflow graph. Each operator has a signature, and the edges carry typed quantities (poses on SE(3), rotations on SO(3), covariances that must stay PSD, residuals that must stay white). The bugs that kill VIO are almost never “the math is wrong in the abstract” — they are contract violations at a boundary: a covariance that lost its cross-correlations, a Jacobian evaluated at the wrong linearization point, a clone augmented with zero cross-covariance, a noise term scaled by Δt where it should be 1/Δt, a calibration treated as perfectly known when it isn’t.

The master post-conditions of the whole filter are three, and everything below exists to make them true:

Master invariant	What it means	Probe
NEES ≈ dim(state)	the filter’s reported covariance matches its actual error magnitude (consistency)	`eval/nav_consistency.hpp` against GT
NIS ≈ dim(measurement)	each innovation is sized as the filter predicted	`eval/consistency.hpp`
Innovations are white	no information left uncaptured frame-to-frame	`eval/innovation_whiteness.hpp`

NEES ≫ dim ⇒ over-confident (our symptom). NEES ≪ dim ⇒ over-conservative. The job of every stage contract below is to be a local check that, if all pass, forces these three globals true.

The canonical pipeline is laid out in dataflow order. For each stage:

Signature — input types → output types.
What it does — the canonical operation (OpenVINS).
Pre — what must hold on the inputs (else the stage is meaningless or unstable).
Post / invariants — what must hold on the outputs (the contract this stage guarantees).
Probes — the measurement devices to instrument it.
Tests + sensitivity — unit tests and the sensitivity sweep that confirms the contract is robust, not coincidental.
cortex — the file that implements it, and candidate divergences from canonical worth checking.

1. The canonical OpenVINS pipeline at a glance

            IMU (200–1000 Hz)                    Camera (20–30 Hz)
                  │                                     │
                  ▼                                     ▼
        ┌───────────────────┐                 ┌───────────────────┐
        │ S2 PROPAGATE      │                 │ S4 FRONTEND       │
        │ mean + cov (Φ,Qd) │                 │ KLT track + RANSAC│
        │ FEJ on Φ          │                 └─────────┬─────────┘
        └─────────┬─────────┘                           │ tracks
                  │ predicted state + P                 ▼
                  ▼                           ┌───────────────────┐
        ┌───────────────────┐  (new image)    │ S5 TRIANGULATE    │
        │ S3 AUGMENT/CLONE  │<────────────────│ lost/marg features│
        │ P' = G P Gᵀ       │                 └─────────┬─────────┘
        └─────────┬─────────┘                           │ p_f, observations
                  │                                     ▼
                  │                            ┌───────────────────┐
                  │                            │ S6 MSCKF UPDATE   │
                  │                            │ H_f,H_x → null-sp │
                  │                            │ → QR compress     │
                  │                            │ → χ² gate         │
                  │                            │ → EKF (Joseph)    │
                  │                            │ FEJ on H_x        │
                  │                            └─────────┬─────────┘
                  ▼                                      │ corrected state + P
        ┌───────────────────┐                            │
        │ S9 MARGINALIZE    │<───────────────────────────┘
        │ drop oldest clone │
        └───────────────────┘
   (S1 INIT bootstraps; S7 SLAM-feat, S8 zero-velocity, S10 online-calib are optional layers)

The state vector OpenVINS actually carries (this is the first place clean-rooms under-build):

x = [ x_IMU(15) | clone_1(6) … clone_n(6) | SLAM_feat_1 … (3 or 1 each) | CALIB ]
                                                                          └─ camera↔IMU extrinsics (6/cam)
                                                                             camera intrinsics+distortion (8/cam)
                                                                             camera↔IMU time offset (1/cam)
                                                                             IMU intrinsics (optional)

x_IMU = [θ_GI (3) | p_GI (3) | v_G (3) | b_g (3) | b_a (3)], error-state, left quaternion multiplicative error in OpenVINS (δq ≈ [½θ; 1]). The cortex implementation uses a world-centric right-SO(3) perturbation (R ← R·Exp(δθ)) — a legitimate alternative convention, but the choice must be globally consistent across propagation, augmentation, update, and the measurement Jacobians. A mixed convention is a classic silent inconsistency.

First load-bearing observation (cortex vs canonical): by default the cortex state is [IMU(15) | clones(6n)] only — no camera intrinsics, extrinsics, or time-offset states. (Since #332, camera↔IMU extrinsic states, 6 per camera, can be enabled with VioConfig::estimate_extrinsics; intrinsics and time offset are still not estimated.) OpenVINS either estimates these online or at minimum its measurement model and noise account for their uncertainty. Treating calibration as perfectly known removes real uncertainty from the filter — which is structurally indistinguishable from over-confidence, and is exactly consistent with the troubleshooting finding that inflating R (×4) restores NEES on MH_05. Inflating R is a crude proxy for the unmodeled calibration/extrinsic/time-offset uncertainty. This is a candidate contract to test (see S0, S10), not a declared fix.

The stages as code (#452)

Each stage below is an explicit transformation in sdk/include/branes/sdk/msckf/stages/, of the shape (state, inputs, params) → (state, diagnostics): the state passed in is the only thing mutated, and what happened is returned rather than logged. MsckfBackendT is their call sequence, and a stage bench calls one in isolation. The stage names carry the transformation.

Stage	Transformation	Header
S0_sensor_model	pixel → normalized image point	`s0_sensor_model.hpp`
S1_initialization	init result → seeded state	`s1_initialization.hpp`
S2_propagation	IMU sample → propagated mean + covariance	`s2_propagation.hpp`
S3_augmentation	state → state + clone	`s3_augmentation.hpp`
S4_frontend	frame observations → tracks, ended tracks	`s4_frontend.hpp`
S5_triangulation	track → feature position	`s5_triangulation.hpp`
S6a_jacobians … S6e_ekf_update	track → updated state, one entry point per sub-step	`s6_msckf_update.hpp`
S8_zero_velocity	IMU + frames at rest → `v = 0`, `b_g = ω̄` pseudo-measurements	`s8_zero_velocity.hpp`
S9_marginalization	state → state − clone	`s9_marginalization.hpp`
S10_online_calibration	state → state + camera↔IMU extrinsic states (6/camera, optional)	`s10_online_calibration.hpp`

S6c_compression is the identity in cortex today (one feature per update leaves 2m − 3 rows), kept as an explicit slot for a compressing variant. S7 is not implemented in cortex.

S0 — Sensor & calibration models (the substrate every stage trusts)

Signature. camera: project(p_c ∈ ℝ³, ζ) → (u,v) ∈ ℝ² and inverse; imu: (ω̃, ã) = model(ω, a, R, g, b, n); extrinsics: T_CI ∈ SE(3), time offset: t_d ∈ ℝ.

What it does. Defines the measurement functions every residual is built from. OpenVINS composes the camera measurement as z = h_d(h_p(h_t(h_r(λ, …), R_CG, p_C)), ζ) + n — four stacked maps: feature representation → world-to-camera transform (uses extrinsics + clone pose) → perspective projection → distortion (radtan 8-param or fisheye/KB 4-param). The IMU model is ω̃ = ω + b_g + n_g, ã = Rᵀ(a − g) + b_a + n_a.

Pre.

ζ valid: f_x, f_y > 0; distortion coefficients within the model’s convergence basin.
Gravity sign and magnitude pinned and frame-consistent. A static IMU must read +‖g‖ along its up axis (it senses the reaction to gravity), not zero.
T_CI and t_d known to some accuracy with a stated uncertainty (this number must feed the filter — see candidate divergence).

Post / invariants.

Round-trip: unproject(project(p_c)) ≈ p_c / ‖p_c‖ within ε for all z > 0. (Distortion models that don’t round-trip are a frontend-poisoning bug.)
Jacobian sanity: ∂h/∂p_c finite and rank-2 for z > 0; matches numerical (finite-diff) Jacobian within ε. (This single check catches a large fraction of homegrown projection bugs.)
Cheirality: features behind the camera (z ≤ 0) are rejected, never projected.
IMU static identity: integrating a stationary IMU through the init orientation yields zero world-frame acceleration.

Probes. Projection/Jacobian round-trip residual; analytic-vs-numeric Jacobian max abs error; static-IMU world-accel magnitude; gravity-magnitude check at init.

Tests + sensitivity.

Unit: random p_c round-trip; analytic Jacobian == finite-diff Jacobian (the workhorse test — every measurement Jacobian in the system gets one).
Sensitivity: time-offset sweep — inject t_d ∈ {−10, −5, 0, +5, +10} ms and measure pose error; quantifies how many mm/° of error per ms of unmodeled offset (this is the cost of not having t_d in the state, directly relevant to over-confidence).
Sensitivity: extrinsic perturbation — perturb T_CI by {0.5°, 1°, 2°} and {1, 5} mm, measure the induced reprojection bias; this is the bias the filter absorbs into the state as if it were noise.

cortex. Camera model + jacobian_at_pose in msckf/camera_updater.hpp:236. IMU model in msckf/propagator.hpp. Candidate divergence: no t_d, no online T_CI, no intrinsics in state — their uncertainty is unmodeled. The two sensitivity sweeps above quantify the resulting covariance deficit.

S1 — Initialization (bootstrap the manifold)

Signature. init(imu_window, [vision_window]) → (R_GI, v_G, b_g, b_a, g, [scale]) + initial P.

What it does. OpenVINS supports static and dynamic init.

Static: detect a stationary window, average accel → gravity direction (fixes roll/pitch, yaw arbitrary), average gyro → b_g, v = 0. Magnitude checked against ‖g‖.
Dynamic: a visual-inertial alignment over a short window solving for gravity, velocity, and scale by matching SfM motion to preintegrated IMU (Dong-Si/Mourikis-style linear system).

Pre.

Static: accel std and gyro std below stationarity thresholds over the window.
Dynamic: sufficient acceleration excitation — scale and accel-bias are weakly observable under gentle or pure-rotation motion (the top real-world init failure). A scale-observability gate must guard this.

Post / invariants.

R_GI aligns measured-accel-mean to gravity: residual gravity after alignment ≈ 0; roll/pitch observable, yaw set to a gauge value.
‖g‖ within tolerance of local gravity (≈ 9.78–9.83).
Dynamic: scale s > 0 and finite; condition number of the alignment linear system bounded (an ill-conditioned system ⇒ scale not observable ⇒ reject, don’t return garbage).
Initial P honestly reflects what was not observed — large variance on yaw, on accel bias, on scale. Over-tight initial P is over-confidence injected at t=0 that the filter never recovers from.

Probes. Post-alignment gravity residual; recovered ‖g‖; alignment-system condition number / smallest singular value; recovered scale vs GT scale; initial-P eigenvalues vs the unobservable directions.

Tests + sensitivity.

Unit: synthetic stationary IMU → recovered R_GI tilts measured gravity onto −z within ε; recovered b_g == injected.
Unit: dynamic init on synthetic excited trajectory recovers injected scale within ε.
Sensitivity: excitation sweep — scale the trajectory’s acceleration by {0.1, 0.3, 1, 3}× and plot recovered-scale error vs excitation; locate the observability cliff. This tells you where init should refuse to fire (and whether yours refuses or returns a confident wrong answer).

cortex. imu_init.hpp (try_static, try_gravity_align, try_dynamic); SfM bootstrap in sfm/two_view.hpp, sfm/pnp.hpp, sfm/init_window.hpp, refined by sfm/bundle_adjust.hpp. The window’s RANSAC gates come from VioConfig::init_track_sigma_px (#500) and the alignment estimates the accelerometer bias (#501); with prefer_dynamic_init over the first 30 s of EuRoC it aligns on 9 of 11 sequences, with a median scale error of 3.6% against ground truth. Candidate divergence: verify initial P carries large yaw / accel-bias / scale variance; an over-tight init P is a first-frame over-confidence source.

S2 — IMU propagation (mean + covariance)

Signature. propagate(x⁻, P⁻, imu[t_k, t_{k+1}]) → (x⁻_{k+1}, P⁻_{k+1}).

What it does — mean. OpenVINS offers a zeroth-order hold (one measurement constant over the interval) or RK4 on the input interpolated between the interval’s two samples; debias, then strapdown-integrate R, v, p. cortex’s decision (#616): RK4 on the interpolated input, in both backends. A sample is the rate at its timestamp, so holding either end of the interval integrates the IMU half a sample (2.5 ms at 200 Hz) early or late against the camera. Until #616 the MSCKF held the end sample and the R-IEKF the start sample, which inflated NIS to 1.1–1.9 on the fast EuRoC sequences. An image between two samples is reached by holding the last sample; the next sample integrates the rest of the interval from the rate interpolated at the image time. The failure mode, and how it hid, is on Aligning Sensor Samples.

What it does — covariance. P⁻_{k+1} = Φ P⁺_k Φᵀ + Q_d, with the canonical block structure (rows/cols [θ, p, v, b_g, b_a]):

Φ = ⎡  R_{k+1,k}              0     0    −R_{k+1,k} J_r(θ) Δt        0        ⎤
    ⎢ −½ R_kᵀ ⌊â Δt²⌋          I   Δt I          0            −½ R_kᵀ Δt²     ⎥
    ⎢ −R_kᵀ ⌊â Δt⌋             0     I            0             −R_kᵀ Δt       ⎥
    ⎢   0                      0     0            I                 0         ⎥
    ⎣   0                      0     0            0                 I         ⎦

The decisive feature is the off-diagonal skew blocks coupling orientation error into velocity and position error — this is gravity-leakage living inside the covariance.

The discrete noise (OpenVINS simplified form): Q_d is built by mapping the measurement-space noise Q_meas = diag(σ_gc²/Δt · I, σ_ac²/Δt · I) through the noise Jacobian G (which carries the J_r Δt, R Δt, ½R Δt² factors — the same structure as Φ’s last columns), plus a bias-walk block Q_bias = diag(σ_wgc² Δt · I, σ_wac² Δt · I). The two noise families scale in opposite directions in Δt: white noise ÷Δt (in measurement space) but maps to ×Δt in angle space; bias walk ×Δt. Getting this backwards silently mis-weights every inertial step.

FEJ. The consistency fix: evaluate Φ (the Jacobian) at the first-estimate linearization point (frozen R_fej, b_fej), while the mean integrates at the current estimate. This must be done consistently with the measurement Jacobian (S6) — half-FEJ (one side only) is worse than none, as the troubleshooting confirmed.

Pre. Δt > 0, monotonic timestamps; measurements debiased with the current bias estimate; P⁺_k symmetric PSD.

Post / invariants.

R_{k+1} ∈ SO(3): orthonormal, det = 1 (re-normalize / use Exp).
P⁻_{k+1} symmetric PSD (eigenvalues ≥ 0).
Q_d PSD and carries the θ–v–p cross-correlations (NOT diagonal).
Observability: the 4-D unobservable subspace (global position + yaw about gravity) lies in the null space of the linearized propagation; with FEJ, Φ does not inject information along yaw. Probe by propagating the analytic nullspace vectors and checking they stay in the null space.
GT-injection identity: with ground-truth IMU and zero noise, propagation reproduces the GT trajectory over a clone window within what the camera resolves there: a tenth of the 0.5 px track noise, 1.09e-4 rad and 0.22 mm at a 2 m depth, converging at second order (tests/tools/gt_spline.cpp, #616). The tolerance comes from the consumer, not from what the integrator achieves: a tolerance fitted to the residual blessed the half-sample skew (#616).

Probes. P min-eigenvalue (PSD monitor); R orthonormality residual ‖RᵀR − I‖; Q_d symmetry/PSD; nullspace-drift metric (‖projection of P⁻¹ onto yaw direction‖); per-step propagation error vs GT (GT-injection harness, #266).

Tests + sensitivity.

Unit: zero-noise GT-injection — propagate against an analytic trajectory, assert mean error and that P grows monotonically along observable directions.
Unit: analytic Φ == finite-diff Jacobian of the mean-propagation map.
Unit: Q_d structure — assert Q_d has nonzero θ–v and θ–p blocks (catches the diagonal-Q bug); assert the Δt-scaling direction by halving Δt and checking white-noise block scales ×½ while bias-walk block scales ×½ in the opposite role.
Sensitivity: noise-density sweep — scale σ_gc, σ_ac by {0.5, 1, 2, 5}× and measure NEES of propagation-only segments; confirms NEES ∝ inverse noise (the lever the sweeps already found).

cortex. msckf/propagator.hpp (Φ assembly :62, Q :81, FEJ :54). Candidate divergence (high priority): the map reports Q assembled as a 12-term diagonal σ²·Δt added directly to the state covariance — i.e. without mapping through the noise Jacobian G. If so, Q_d is missing the θ–v–p cross-correlations and the higher-order Δt²/Δt³ velocity/position scalings that the canonical G Q_meas Gᵀ produces. A diagonal Q_d understates position/velocity process noise and omits the correlations the update relies on — a direct over-confidence mechanism. The “Q_d structure” unit test above is designed to catch exactly this. Verify the Φ velocity-row sign and the J_r term too (the R_{k+1,k} J_r Δt block in the θ–b_g coupling is frequently dropped in clean-rooms).

S3 — State augmentation / cloning

Signature. augment(x, P) → (x', P') adding a clone of the current camera pose.

What it does. Stochastic cloning: append the current IMU pose (transformed by extrinsics to the camera pose, or the IMU pose with T_CI applied at measurement time) as a new state, with P' = G P Gᵀ where G = [I; J] and J maps the existing IMU pose error into the new clone’s error. The new clone’s pose block is a copy of the current pose block.

Pre. clone count < window max; current pose valid; P PSD.

Post / invariants — the contract clean-rooms most often break.

P' symmetric PSD.
Immediately after cloning, the clone’s marginal covariance equals the cloned pose’s marginal, and the cross-covariance between the clone and every other state equals that of the cloned pose. The clone is a deterministic copy — it is perfectly correlated with its source, not independent. Augmenting with a zero or block-diagonal cross-covariance (“just add a new block”) is a textbook inconsistency that makes the filter under-confident at clone time and corrupts every subsequent update that spans clones.
G carries the right convention: if the clone is the camera pose, J must include ∂(camera pose)/∂(IMU pose) through T_CI (and through t_d if modeled).

Probes. Block-equality assertion ‖P'[clone,clone] − P[pose,pose]‖ and ‖P'[clone,*] − P[pose,*]‖ (must be ≈ 0 at augmentation); P' min-eigenvalue.

Tests + sensitivity.

Unit: after augment, assert the two block-equality invariants to machine precision.
Unit: augment then immediately marginalize the clone ⇒ P returns to the original (round-trip).
Sensitivity: if extrinsics enter G, perturb T_CI and confirm the cross-covariance changes as predicted (guards the extrinsic Jacobian in G).

cortex. msckf/state_helper.hpp:31 (augment_clone, G = [I; J], cov.transform(G)). The map shows J mapping onto IMU δθ, δp — verify whether T_CI is included (if the clone is meant to be the camera pose) or whether clones are IMU poses with extrinsics applied later in the measurement Jacobian. Either is valid; mixing them is not.

S4 — Visual frontend (track generation)

Signature. track(image_t, prev_tracks) → observations[(id, cam, u, v)].

What it does. FAST/Shi-Tomasi detection with grid-bucketed spatial coverage and min-distance suppression; pyramidal KLT (or descriptor matching) frame-to-frame; forward-backward check; RANSAC on the epipolar/fundamental constraint. The IMU gyro prior can seed KLT search and reduce RANSAC to 2-point.

Pre. image in expected format; previous tracks available; (optionally) gyro rotation prior.

Post / invariants.

Every surviving track passes forward-backward (round-trip pixel error < threshold).
RANSAC inlier ratio above a floor; outliers removed before the backend sees them (no backend trick repairs a confidently-wrong correspondence).
Spatial coverage: features distributed across the image grid (cluster → poorly-conditioned rotational DoF).
Track-length distribution healthy (not all length-2 — that starves parallax).

Probes. Per-frame: track count, inlier ratio, grid-occupancy histogram, forward-backward residual distribution, track-length histogram, parallax distribution.

Tests + sensitivity.

Unit: synthetic translating/rotating pattern — recovered tracks match the warp within ε.
Unit: inject known outliers — RANSAC + FB rejects them.
Sensitivity: pixel-noise injection — add σ ∈ {0.1, 0.5, 1, 2} px to features and measure track survival and downstream reprojection residual; calibrates the true measurement noise the backend should use (vs the assumed pixel σ).

cortex. Frontend in vio_estimator.hpp:162 (FrontendParams, KLT in tests/cv/klt_tracker.cpp). Candidate divergence: confirm forward-backward + RANSAC are both active and that the pixel σ fed to S6 matches the measured track noise from the sensitivity sweep (not an optimistic constant).

S5 — Feature triangulation

Signature. triangulate(observations, clone_poses, T_CI) → (p_f ∈ ℝ³, status).

What it does. Linear (ray-perpendicular / DLT) triangulation across the observing clones, then Gauss-Newton refinement (inverse-depth parameterization is best-conditioned for far points). OpenVINS triangulates at the current clone poses (best geometry).

Pre. ≥ 2 observations from distinct, sufficiently-parallax clones; cheirality holds. Under pure rotation parallax → 0 and nothing triangulates — the frontend must flag these, not feed them in.

Post / invariants.

Reprojection residual at the solution below threshold (else the track is an outlier that survived RANSAC, or the geometry is degenerate).
Depth positive and finite; cheirality holds in every observing view.
Condition number of the triangulation normal matrix bounded — low parallax ⇒ ill-conditioned ⇒ huge depth uncertainty ⇒ the feature must be deferred or down-weighted, not used as if precise.

Probes. Per-feature: parallax angle (max over view pairs), reprojection RMS at solution, triangulation condition number / depth standard deviation, cheirality pass rate.

Tests + sensitivity.

Unit: synthetic feature seen from known poses → recovered p_f within ε; reprojection ≈ 0.
Unit: degenerate (pure-rotation) input ⇒ triangulation refuses (status = degenerate), not a confident wrong depth.
Sensitivity: parallax sweep — vary baseline so parallax spans {0.1°, 0.5°, 2°, 10°} and plot depth error + depth uncertainty vs parallax; locate the threshold below which features should be excluded. A feature triangulated at 0.2° parallax and then used at full weight is a covariance-corruption source.

cortex. msckf/camera_updater.hpp:272 (linear triangulation :275, GN refine :301). Candidate divergence: verify there is a parallax / condition-number gate before a triangulated feature is admitted to the update. Admitting low-parallax features at full weight injects optimistic information — a plausible contributor to the aggressive-motion (V2_03) divergence where parallax is erratic.

S6 — MSCKF update (the consistency-critical stage)

Signature. update(x⁻, P⁻, {feature tracks}) → (x⁺, P⁺).

This is one stage but five sub-contracts. The order is: build (H_f, H_x, r) → null-space project out the feature → QR-compress H_x → χ²-gate → EKF update.

S6a — Feature & state Jacobians, residual

r = z − ẑ, r ≈ H_x δx + H_f δf + n. H_x is nonzero only on the observing clone columns (and on extrinsics/intrinsics if calibrated). FEJ: H_x and H_f are evaluated at the first-estimate clone poses (R_fej, p_fej); the residual r uses the current poses.

Pre: triangulated p_f valid (S5); ≥ 2 observations.
Post: H_f has full column rank 3 (else the feature direction is unobservable and null-space projection is rank-deficient); analytic H_x, H_f == finite-diff within ε; H_x sparsity correct (zero off the observing clones).
Probe: rank(H_f); analytic-vs-numeric Jacobian error; H_x nonzero-pattern check; FEJ-divergence ‖R_fej − R‖, ‖p_fej − p‖ per clone (the troubleshooting already built this).

S6b — Left null-space projection (marginalize the feature)

Choose N spanning the left null space of H_f (Nᵀ H_f = 0); left-multiply: Nᵀ r = Nᵀ H_x δx + Nᵀ n. Feature eliminated algebraically.

Pre: m ≥ 2 observations ⇒ 2m ≥ 4 > 3 = dim(feature) so the null space is non-trivial.
Post: projected residual has dimension 2m − 3; N is orthonormal (NᵀN = I) so the projected noise stays σ² I (this is why you can keep the scalar noise — break orthonormality and the noise model is silently wrong).
Probe: ‖Nᵀ H_f‖ (should be ≈ 0); ‖NᵀN − I‖ (orthonormality); output row count == 2m−3.
Test: synthetic H_f → assert Nᵀ H_f ≈ 0 and NᵀN = I; assert dimension.

S6c — Measurement (QR/Givens) compression

Thin-QR the tall stacked H_x = Q₁ R₁; keep R₁ (state-sized) and Q₁ᵀ r. Purely for speed (reduces the EKF matrix multiply). Noise stays isotropic because Q₁ᵀ is orthogonal.

Post: Q₁ᵀ orthogonal ⇒ noise still σ² I; the compressed system has the same information as the uncompressed (innovation and gain identical up to the rotation).
Probe: assert update result is identical with/without compression on a test case (it must be — compression is exact, not an approximation).
Note (cortex): the map did not find a second QR compression stage. This is an efficiency feature, not a correctness one — its absence does not cause over-confidence. Flagged only for completeness; do not chase it for #212.

S6d — Chi-square (Mahalanobis) gating

γ = rᵀ (H P⁻ Hᵀ + R)⁻¹ r; accept if γ < χ²_{thresh}(dof). Rejects outliers and linearization-failure measurements.

Pre: innovation covariance S = H P⁻ Hᵀ + R well-conditioned (Cholesky succeeds).
Post: accepted innovations satisfy γ ~ χ²(dof) — i.e. NIS is distributed as χ² with the right dof. NIS systematically ≫ dof ⇒ S too small ⇒ R and/or P under-stated (the over-confidence signature, measured already: NIS ∝ 1/R²).
Probe: NIS distribution vs χ²(dof) bounds (eval/consistency.hpp); gate rejection rate; cond(S).
Test + sensitivity: with GT-consistent noise, NIS time-average ≈ dof. Gate-threshold sweep — vary the χ² multiplier and watch ATE vs rejection rate; a too-tight gate that rejects good aggressive-motion measurements is a V2_03-divergence suspect (the filter stops correcting exactly when motion is hardest).

S6e — EKF update (apply correction)

K = P⁻ Hᵀ S⁻¹; δx = K r; x⁺ = x⁻ ⊞ δx; covariance update Joseph form P⁺ = (I−KH) P⁻ (I−KH)ᵀ + K R Kᵀ.

Post: P⁺ symmetric PSD (Joseph guarantees this even with numeric error); x⁺ on-manifold (R ⊞ δθ via Exp, never additive on rotation); FEJ linearization points (R_fej) not touched by the update.
Probe: P⁺ min-eigenvalue; ‖RᵀR − I‖ post-update; trace(P) trend (should drop on update, grow on propagation — a monotone-decreasing trace across both is over-confidence).
Test: Joseph form keeps P PSD under a stress sequence; x ⊞ δx retraction correctness.

The master post-conditions of S6 as a whole: post-update NEES ≈ dim(state), NIS ≈ dof, innovations white. These are the three globals from §0 — S6 is where they are won or lost.

cortex. msckf/camera_updater.hpp:130 (update), null-space in features/msckf_nullspace.hpp:42, EKF + Joseph in msckf/covariance.hpp:106, gating :143. FEJ toggle per-track :164. The structure is faithful; the defect (if in S6) is most likely in the noise scaling / what R and P⁻ actually contain entering S — which points back to S2’s Q_d and S0/S10’s unmodeled calibration, not to the null-space/Joseph algebra (which the map shows are textbook-correct).

S7 — SLAM-feature update (optional, persistent landmarks)

Signature. features kept in-state (3 or 1 params each, anchored inverse-depth), updated by reprojection without marginalization; long-lived landmarks bound drift better than pure-MSCKF.

Post / invariants. in-state features get P blocks that must stay PSD and correlated with the poses that see them; feature initialization into the state must use stochastic init (correct cross-covariance), the same contract as S3. Pruning/marginalizing a SLAM feature = Schur complement, info-preserving.

cortex. features/representations.hpp defines the parameterizations; not the current focus — the running filter is pure-MSCKF. Listed for pipeline completeness.

S8 — Zero-velocity update (optional, robustness)

Signature. detect stationarity (IMU + visual disparity) → apply a v = 0 pseudo-measurement (and suppress clone insertion while still).

Post / invariants. prevents spurious motion / feature-starved drift while static; the detector must not false-positive during slow motion (would inject a wrong v=0 constraint → bias).

cortex. msckf/stages/s8_zero_velocity.hpp (#600), on by default (VioConfig::zero_velocity_update). The detector requires both the IMU’s test (the gyro rate variation below a 20 Hz low-pass, ≤ 0.008 rad/s over 0.25 s) and the image’s (median feature displacement over 5 frames ≤ 1 px). At rest the backend applies v = 0 (σ 0.01 m/s) each frame, b_g = ω̄ (σ 0.001 rad/s) once per 0.25 s IMU window, and adds no clone. On the 11 EuRoC sequences it fired at none of the 24,353 frames whose ground truth moves faster than 0.05 m/s, and at 47–78 % of the frames at rest; that is a measurement on those runs, not a guarantee. The ROC and static-drift probes listed above are not yet written.

S9 — Marginalization / clone management

Signature. marginalize(x, P, clone_idx) → (x', P').

What it does. Slide the window: drop the oldest clone (and features anchored to it that aren’t promoted). In covariance form this is principal-submatrix extraction of the kept states; in information form it’s a Schur complement that produces fill-in.

Pre. clone selected by policy (oldest, or VINS-Mono two-way: keyframe vs non-keyframe).

Post / invariants.

P' symmetric PSD; the kept-state marginal is unchanged by removing a clone (marginalizing a variable does not alter the marginal of the others — a correctness check on the extraction).
The marginalization (in a smoother, the prior) must not constrain the 4 gauge directions.
For a filter doing principal-submatrix extraction, no FEJ issue arises here (unlike a smoother’s marginalization prior); but the augment (S3) cross-covariance must have been correct or the extracted marginal is already wrong.

Probes. P' PSD; kept-marginal-invariance ‖P'[keep] − P[keep,keep]‖ (must be 0 for pure extraction); window length bound.

cortex. msckf/state_helper.hpp:51 (marginalize_clone, principal submatrix via cov.marginalize(keep)). The round-trip test in S3 covers augment∘marginalize.

S10 — Online calibration (extrinsics, intrinsics, time offset)

Signature. calibration parameters as state: T_CI (6/cam), intrinsics+distortion (8/cam), t_d (1/cam), optional IMU intrinsics.

What it does. OpenVINS estimates these online; their Jacobians enter H_x (S6a). Even when not estimated, the filter must account for their uncertainty (inflate R, or carry a fixed prior block) — otherwise it treats imperfect calibration as perfect and is over-confident.

Post / invariants. if estimated: calibration states observable only under sufficient excitation (like accel bias); if fixed: the assumed-perfect calibration’s true uncertainty must appear somewhere in the noise budget.

Probes. time-offset and extrinsic sensitivity sweeps (S0); if estimated, calibration-state NEES vs GT calibration.

cortex — candidate divergence (high priority, ties to #212). No calibration states and (per the map) no calibration-uncertainty term in the measurement noise. The EuRoC T_CI is taken as truth. The troubleshooting found honest noise needs R ×4 to fix NEES on tracked sequences — R ×4 is numerically what unmodeled extrinsic + time-offset + intrinsic uncertainty would require. Test directly: add a fixed extrinsic/time-offset uncertainty block (or the equivalent R inflation derived from the S0 sensitivity sweeps) and re-measure NEES/NIS/ATE. If NEES drops toward dim without the blunt global R ×4, the over-confidence is unmodeled-calibration, not a filter-algebra bug.

Result (measured 2026-06-11, §1 candidate #1). This was built (calib_rot_sigma, an isotropic extrinsic-rotation R-term) and swept on V2_03. The innovation responds exactly as predicted — NIS 14.7 → 1.41 as the term grows — confirming R was understated. But NEES and ATE regress (the aggressive-motion filter is starved of vision and dead-reckons), so a fixed isotropic term is refuted as the fix for the same reason the global R×4 is a crude proxy. The remaining principled path is estimating the calibration online (T_CI/t_d as state), which corrects the bias rather than down-weighting the signal — not a uniform noise inflation.

2. Cross-system contrast — what each reference changes per stage

Stage	OpenVINS (filter / MSCKF)	MINS (OpenVINS++)	VINS-Mono/Fusion (smoother)	ROVIO (direct filter)	ORB-SLAM3 (smoother)
Backend	EKF, MSCKF null-space + optional in-state SLAM feats	same EKF core, multi-sensor	sliding-window NLS (Ceres)	iterated EKF, photometric	local BA + factor graph (g2o/GTSAM-like)
Frontend	KLT or descriptor	KLT	KLT	direct patches (no features)	ORB descriptors
Feature handling	null-space marginalize	same	reprojection factors, inverse-depth	patch intensity in EKF state	map points, covisibility
IMU	preintegration not needed (propagate each step)	same	preintegration factors	propagate each step	preintegration
Consistency	FEJ	FEJ + consistent interpolation	marginalization prior + FEJ	iterated EKF	marginalization
Online calib	extrinsics, intrinsics, time offset, IMU intrinsics	full spatiotemporal, all sensors	extrinsics + time offset	extrinsics	extrinsics
Loop closure	none (odometry)	optional pose-graph	4-DoF pose graph + DBoW2	none	full (DBoW2 + global BA)
Init	static + dynamic	proprioceptive-only dynamic	SfM + VI alignment	filter converges from rough	IMU + SfM, robust
Distinguishing idea	clean, well-documented MSCKF; the reference filter	async multi-sensor via high-order on-manifold interpolation + dynamic cloning	preintegration + marginalization smoother	photometric, no frontend	full SLAM, relocalization

Reading for cortex: the architecture in the repo is squarely the OpenVINS column — correct choice for the compute-constrained KPU target, and the right reference to match stage-for-stage. The smoother columns (VINS/ORB) differ in backend but share S0/S1/S4/S5 contracts and the same observability/consistency invariants — so the contracts above transfer if the backend is ever swapped.

3. MINS — what it adds on top of the OpenVINS core (for later phases)

MINS (Lee, Geneva, Chen, Huang — JFR 2025; github.com/rpng/MINS, arXiv:2309.15390) keeps the OpenVINS EKF/MSCKF core and generalizes it to robust asynchronous multi-sensor fusion. The ideas worth importing as contracts, even for a camera-only system:

High-order on-manifold interpolation for asynchronous measurements. Instead of requiring a clone exactly at each measurement time, MINS interpolates poses on SE(3) between clones to the measurement timestamp. Contract relevance: this is the principled version of handling the camera↔IMU time offset — the same t_d whose unmodeled uncertainty we flagged in S0/S10.
Dynamic cloning — adaptively choosing interpolation order / clone density from motion-induced information. Contract relevance: a measurement-driven version of keyframe selection; ties to the parallax/conditioning gate in S5.
Full online spatiotemporal calibration for every sensor (extrinsics + intrinsics + time offset, per sensor). Contract relevance: directly the S10 gap — MINS treats calibration uncertainty as a first-class estimated quantity rather than assuming it away.
Proprioceptive-only initialization and explicit robustness/consistency (FEJ throughout).
Optional loop closure via a separate pose-graph layer (turns odometry → SLAM).

For our immediate problem, the MINS lesson is #3: a consistent system models calibration uncertainty rather than trusting datasheet/dataset extrinsics — reinforcing the S10 candidate.

4. Consolidated contract / probe matrix

The build order: implement the probe, write the test, confirm the contract in isolation, then compose. Probes that already exist in sdk/include/branes/sdk/eval/ are marked ✓.

Stage	Key invariant (post-condition)	Probe	Exists?
S0 model	proj round-trip; analytic==numeric Jacobian	jacobian-diff harness	new
S0 calib	pose error per ms `t_d`, per ° extrinsic	sensitivity sweep	new
S1 init	gravity residual≈0; ‖g‖ ok; init `P` honest on yaw/scale/b_a	init-P eigen check	partial
S2 mean	`R∈SO(3)`; GT-injection error bounded	orthonormality; GT harness (#266)	partial
S2 cov	`P` PSD; `Q_d` has θ–v–p cross terms; nullspace preserved	PSD monitor; Q-structure test	new
S3 augment	clone marginal==pose marginal; cross-cov==pose cross-cov	block-equality assert	✓ (tested, residual 0)
S4 frontend	FB-consistent; coverage; inlier ratio	track stats; pixel-noise sweep	partial
S5 triangulate	reproj<thresh; depth>0; parallax-gated	parallax / cond-number probe	✓ (tested; no soft gate on admit path)
S6a Jacobian	`rank(H_f)=3`; analytic==numeric; FEJ divergence	jacobian-diff; FEJ-divergence log	✓ (FEJ log)
S6b nullspace	`NᵀH_f≈0`; `NᵀN=I`; rows=2m−3	orthonormality assert	✓ (tested, residual ~2e-16)
S6c compress	exact (result == uncompressed)	equivalence test	n/a (absent)
S6d gate	NIS ~ χ²(dof)	`eval/consistency.hpp`	✓
S6e update	`P⁺` PSD (Joseph); on-manifold; FEJ frozen	PSD monitor	✓
filter	NEES≈dim, NIS≈dof, innovations white	`eval/nav_consistency.hpp`, `innovation_whiteness.hpp`	✓
S9 marg	`P'` PSD; kept-marginal invariant	extraction-invariance assert	✓ (tested, residual 0)
S10 calib	calibration uncertainty represented	extrinsic/`t_d` sensitivity; calib-NEES	new

5. Where the running filter diverges from canonical

The ranking of where cortex’s filter departs from the canonical pipeline, and what each measurement settled, is kept current on Engineering Status and Consistency Analysis.

6. Running the contracts: the stage-probe utilities

The contracts above are executable. Each stage S0–S10 has a runnable study utility under tools/ — not a test and not a daemon, but a tool you run to focus on one stage in isolation, perturb an input, and watch a problem-native metric move:

cmake -B build -DBUILD_TARGET_KPU=OFF && cmake --build build -j$(nproc)
./build/tools/s0_sensor_model --help      # print this stage's contract
./build/tools/s0_sensor_model --list      # the whole S0–S10 pipeline
./build/tools/s0_sensor_model --out build/stage_probes/S0   # run + write artifacts
node docs-site/scripts/gen-sensor-model-figures.mjs build/stage_probes/S0 docs/assessments/figures/s0

The architecture: the probe computation lives in header-only, type-generic sdk/include/branes/sdk/eval/*_probe.hpp (shared by the utility and the regression test in tests/sdk/sensor_model_probe.cpp); the contracts are a single machine-readable registry in tools/include/branes/tools/vio_stage_contracts.hpp; each tools/src/sN_*.cpp is a thin driver that prints the contract, runs the probes, prints a native-unit results table, and writes the CSV artifacts the figure generator renders.

Each stage’s driver prints its contract and a native-unit results table; what each probe measured is on that stage’s page (for example S0, S6), and the overall verdict on Engineering Status.

End-to-end noise→robustness demo

Beside the per-stage probes, tools/src/vio_pipeline.cpp runs the whole pipeline as a stream: a synthetic world with exact ground truth → an additive-noise injector → the real MSCKF backend → an estimate stream, measuring how well the filter rejects the noise; or a recorded EuRoC sequence, without the injector, → the same backend. Running the Demo shows how to run it and read its output.

Sources

OpenVINS documentation — propagation, discrete propagation, feature update, measurement compression, system overview: https://docs.openvins.com/
Mourikis & Roumeliotis, A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation, ICRA 2007 (MSCKF).
Geneva, Eckenhoff, Lee, Yang, Huang, OpenVINS: A Research Platform for Visual-Inertial Estimation, ICRA 2020.
Huang, Mourikis, Roumeliotis — observability-constrained EKF / First-Estimate Jacobians.
Forster, Carlone, Dellaert, Scaramuzza, On-Manifold Preintegration, TRO 2017.
Lee, Geneva, Chen, Huang, MINS: Efficient and Robust Multisensor-aided Inertial Navigation System, JFR 2025 — https://arxiv.org/abs/2309.15390, https://github.com/rpng/MINS.
Companion: docs/arch/visual-inertial-odometry.md; the FEJ measurements, docs/sessions/2026-06-04-vio-troubleshooting-fej.md.

Licensing note. OpenVINS and MINS are GPL. This distillation is derived from their documentation and the underlying papers (algorithmic/architectural level), not from their source — consistent with the repo’s clean-room-from-papers rule.