S6 — MSCKF Update

This is the stage that turns feature reprojection residuals into a state correction — and it is the consistency-critical stage. The three global health properties of the whole filter (NEES ≈ state dimension, NIS ≈ measurement dof, white innovations — see Reading the Metrics) are won or lost here. It is one stage, but five sub-contracts, and the order matters.

What it does

For each feature track that is being marginalized (lost, or sliding out of the window), S6 builds the reprojection residual, algebraically removes the unknown 3-D feature, gates the result, and applies an EKF correction to the whole sliding window:

build (H_x, H_f, r)  →  null-space project out the feature  →  QR-compress
                     →  χ²-gate  →  EKF update (Joseph)

The five sub-contracts

S6a — Jacobians & residual

r = z − ẑ, linearized as r ≈ H_x δx + H_f δf + n. H_x is nonzero only on the columns of the observing clone(s). First-Estimates Jacobians (FEJ): H_x, H_f are evaluated at the clones’ first-estimate poses while the residual uses the current poses — the standard fix for the MSCKF’s observability inconsistency. Invariants: rank(H_f) = 3; analytic Jacobians match finite-difference; H_x sparsity correct.

S6b — Left null-space projection

Pick N spanning the left null space of H_f (Nᵀ H_f = 0) and left-multiply: the feature is eliminated algebraically, leaving Nᵀ r = Nᵀ H_x δx + Nᵀ n. Invariant: N is orthonormal (NᵀN = I) so the projected noise stays σ² I — break orthonormality and the noise model is silently wrong. Output dimension is 2m − 3 for m observations.

S6c — QR compression

Thin-QR the tall H_x and keep the state-sized factor. Purely for speed, and exact (the orthogonal Q₁ᵀ keeps the noise isotropic and the information identical). Its absence is an efficiency matter, never a correctness one — not a #212 suspect.

S6d — χ² (Mahalanobis) gating

γ = rᵀ S⁻¹ r with S = H P⁻ Hᵀ + R; accept if γ < χ²_thresh(dof). Invariant: accepted innovations satisfy γ ~ χ²(dof) — i.e. NIS is χ²-distributed with the right dof. NIS systematically ≫ dof ⇒ S too small ⇒ R and/or P understated: the over-confidence signature (measured: NIS ∝ 1/R²). A too-tight gate that rejects good aggressive-motion measurements is a divergence suspect — the filter stops correcting exactly when motion is hardest.

S6e — EKF update (Joseph form)

K = P⁻ Hᵀ S⁻¹, δx = K r, x⁺ = x⁻ ⊞ δx, and the Joseph-form covariance update P⁺ = (I−KH) P⁻ (I−KH)ᵀ + K R Kᵀ. Invariants: P⁺ symmetric PSD (Joseph guarantees it under numeric error); x⁺ retracted on-manifold (R ⊞ δθ via Exp, never additive on rotation); FEJ linearization points untouched by the update.

The contract

Signature	update(x⁻, P⁻, {feature tracks}) → (x⁺, P⁺)
Pre	triangulated p_f valid (S5), ≥ 2 observations per track; S well-conditioned (Cholesky succeeds)
Master post-conditions	NEES ≈ dim(state) · NIS ≈ dof · innovations white — the three globals; S6 is where they are won or lost
cortex	msckf/camera_updater.hpp (update), features/msckf_nullspace.hpp (null-space), msckf/covariance.hpp (Joseph + gating), per-track FEJ toggle

Metrics

• NIS (always-on, no ground truth) — the χ² consistency of each accepted update; the live over-confidence signal.
• Gate rejection rate and cond(S) — too-tight gating vs ill-conditioning.
• Joseph PSD — P⁺ min-eigenvalue ≥ 0 under a stress sequence; ‖RᵀR − I‖ after the on-manifold retraction.
• FEJ divergence — ‖R_fej − R‖, ‖p_fej − p‖ per clone, to confirm FEJ is actually engaging.

Measured — the update algebra is consistent, not just plausible

The S6 probe drives the shipped CameraUpdater::update in isolation on a self-consistent scene: each trial perturbs the clone poses by a draw from the filter’s own covariance P and adds image noise of exactly the σ the filter assumes. Under correct linearization and an honest (P, R) the projected innovation is χ²(2m−3), so NIS/dof must converge to 1 — if the shipped code mis-scaled R, mis-built H, or broke the null-space noise structure, it would not. It does (4 clones ⇒ dof = 5; 4000 Monte-Carlo updates):

measurement	result	meaning
‖NᵀH_f‖ (feature marginalized)	2.8 × 10⁻¹⁶	the null-space eliminates the feature exactly
‖NᵀN − I‖ (orthonormality)	2.2 × 10⁻¹⁶	reflectors orthonormal ⇒ projected noise stays σ²·I
projected residual dimension	5 = 2m−3	correct compressed dimension
NIS / dof @ matched (P, R)	1.002 (band [0.98, 1.02])	the update algebra is consistent
Joseph keeps P⁺ PSD	yes, every update	the covariance update is numerically sound

The decisive row is NIS/dof = 1.00: fed honest inputs, the shipped triangulation, Jacobians, null-space projection, innovation covariance S, and Joseph update produce innovations sized exactly as predicted. So the #212 over-confidence is not in the S6 math — it enters through the inputs P⁻ and R.

The noise-mismatch sweep makes that lever explicit — hold the modeled R fixed and inject noise at a multiple of the assumed σ:

injected noise (× assumed σ)	NIS / dof
0.5×	0.84
1× (matched)	1.00
2×	1.64
4×	4.17

NIS climbs above dof exactly as the true measurement noise outgrows the modeled R — the innovation-level image of the empirical “R×4” that S10 attributes to unmodeled calibration.

Real-data inspector (s6_inspect)

The measurements above come from the synthetic probe — a self-consistent (P, R) scene, run in CI. Its real-data companion, s6_inspect, establishes the filter-internal renderer tier (epic #371): it runs the real estimator over a real EuRoC sequence and taps every MSCKF camera update through a backend observer (the #380 decouple — MsckfBackend::set_update_observer, a no-op in production), recording the update the monolithic backend otherwise hides.

s6_inspect --dataset /path/to/V1_01_easy/mav0 --out build/s6
#   --camera-noise σ   --calib-rot-deg D   --min-parallax DEG   --dump-update K
node docs-site/scripts/gen-update-figures.mjs build/s6   # → nis.svg, residuals.svg, covariance.svg

Per update it records the NIS / dof, the χ²-gate decision (accept / reject), the per-observation reprojection residual, and the covariance trace before / after (and whether Joseph kept P PSD). The figures draw the NIS-over-updates curve against its χ² consistency band — points coloured by the gate decision — the residual-RMS series, and the covariance before/after heatmap (log|P|, with the IMU/clone blocks marked). Because it reads the live filter covariance, this is where the #212 over-confidence shows up directly: a NIS curve riding above the band while the covariance keeps shrinking is the filter trusting vision more than the data supports.

Unlike the synthetic probe (which proves the algebra is consistent on a matched (P, R)), s6_inspect measures the update on the real P⁻/R the pipeline actually produces — so over-confidence sourced upstream (S2 process noise, S0/S10 calibration) becomes visible at the innovation. Sweeping --camera-noise / --calib-rot-deg moves the NIS curve into the band, the real-data analogue of the “R×4” compensation.

Interactive

The NIS readout in the 3-D viewer is this stage’s health, live: it is the per-update χ² of S6d. When it climbs past 1 while the covariance ellipsoid stays small, S6’s post-conditions are being violated.

Status — algebra measured-consistent; over-confidence is upstream of S6

The S6 algebra is measured-consistent, not merely plausible: on a self-consistent (P, R) scene the shipped update returns NIS/dof = 1.00 with machine-zero null-space residuals and PSD-preserving Joseph updates. The defect behind the #212 over-confidence is therefore not the S6 math but what R and P⁻ contain entering S — which points back to S2 process noise and S0 / S10 unmodeled calibration. Notably, the S6a FEJ fix — the prime textbook suspect — was implemented and refuted by measurement. Not yet instrumented here: the analytic-vs-numeric Jacobian check and FEJ-divergence telemetry (the current updater linearizes at the live poses). The full diagnostic chain is on Consistency Analysis; the live decomposition on Engineering Status.