Why
The #212 structural over-confidence is now localized by measurement to a yaw observability leak (the observability_probe, #337): the standard EKF's Jacobians depend on the state estimate, so re-linearizing at the evolving estimate injects spurious information into the 4-DoF unobservable subspace (global translation + gravity-yaw), and the yaw covariance is wrongly shrunk — the measured attitude-NEES ≈ 993 on slow V1_01 (NIS a healthy 1.5). The textbook patch is FEJ; both a measurement-only and a full propagation-Φ FEJ were built and measurably diverge (V1_01 →2281 m, MH_05 →11 km — canonical §1 candidate #5, parked feat/fej-*). FEJ is treacherous to implement and a forever-frozen first-estimate trajectory dead-reckons to garbage.
The structural cure
The Right-Invariant EKF (R-IEKF) (Barrau & Bonnabel) defines the estimation error on the Lie group instead of as a vector difference. For group-affine dynamics the error dynamics become state-independent — F/H are constant matrices (gravity, ω, dt; not the estimate). With no estimate in the Jacobian there is no linearization point to get wrong: the unobservable subspace is preserved by construction, FEJ is unnecessary, and yaw NEES cannot blow up. It is the correct geometry for INS, not a patch.
Design (cortex)
- State on
SE₂(3) (extended pose): group (R, v, p); biases (b_g, b_a) as a vector → the standard imperfect-IEKF first-order treatment.
- Right-invariant error
η = X̂ X⁻¹ (world-frame) — decouples gravity/yaw and composes with cortex's world-centric convention.
- Propagation: IMU kinematics in group-affine form;
Φ is the constant SE₂(3) structure matrix (no R[a]× at the estimate — the term that leaked yaw).
- Camera update: the MSCKF machinery (clones on the group, triangulation, left-null-space marginalization, χ² gate, Joseph) unchanged in spirit; re-derive
H w.r.t. the invariant error.
- Parallel backend: add
MsckfInvariantBackend<T> alongside MsckfBackend/MsckfSqrtBackend behind the existing VioBackend + Cov-policy seams — the shipped EKF stays default; the two are A/B-compared on the same EuRoC harness. Do not mutate the shipped filter.
Acceptance gate (already built)
Extend observability_probe.hpp to the invariant error and confirm ‖H·N‖_yaw ≈ 0 even at a perturbed estimate (the invariant H is estimate-independent, so the leak the standard EKF showed — 2e-16 → 0.15 — stays at machine-ε for any σ). System gate: EuRoC attitude-NEES → ~1, no divergence. This is the gate the hand-rolled FEJ lacked.
Phasing
- Phase A (spike, de-risking):
SE₂(3) invariant error + group propagation + the invariant observability probe (yaw-leak → 0 by construction). If the probe doesn't go flat, stop. → separate spike issue.
- Phase B: invariant camera measurement update (re-derive
H); unit + recovery tests.
- Phase C: EuRoC verdict across the motion spectrum (attitude-NEES → ~1).
Risk & alternatives
- Biggest change in the program; the parallel-backend isolation makes it safe.
- Lighter fallbacks if scope is a concern: OC-EKF (project
F/H onto 𝒩⊥ each step — a bounded patch on the current EKF) or the Equivariant Filter (EqF). R-IEKF is the recommended robust answer; OC-EKF is the cheaper consistency patch.
Localizes / supersedes the FEJ approach for #212. Refs #337, #339, #212.
Why
The #212 structural over-confidence is now localized by measurement to a yaw observability leak (the
observability_probe, #337): the standard EKF's Jacobians depend on the state estimate, so re-linearizing at the evolving estimate injects spurious information into the 4-DoF unobservable subspace (global translation + gravity-yaw), and the yaw covariance is wrongly shrunk — the measured attitude-NEES ≈ 993 on slow V1_01 (NIS a healthy 1.5). The textbook patch is FEJ; both a measurement-only and a full propagation-Φ FEJ were built and measurably diverge (V1_01 →2281 m, MH_05 →11 km — canonical §1 candidate #5, parkedfeat/fej-*). FEJ is treacherous to implement and a forever-frozen first-estimate trajectory dead-reckons to garbage.The structural cure
The Right-Invariant EKF (R-IEKF) (Barrau & Bonnabel) defines the estimation error on the Lie group instead of as a vector difference. For group-affine dynamics the error dynamics become state-independent —
F/Hare constant matrices (gravity, ω, dt; not the estimate). With no estimate in the Jacobian there is no linearization point to get wrong: the unobservable subspace is preserved by construction, FEJ is unnecessary, and yaw NEES cannot blow up. It is the correct geometry for INS, not a patch.Design (cortex)
SE₂(3)(extended pose): group(R, v, p); biases(b_g, b_a)as a vector → the standard imperfect-IEKF first-order treatment.η = X̂ X⁻¹(world-frame) — decouples gravity/yaw and composes with cortex's world-centric convention.Φis the constantSE₂(3)structure matrix (noR[a]×at the estimate — the term that leaked yaw).Hw.r.t. the invariant error.MsckfInvariantBackend<T>alongsideMsckfBackend/MsckfSqrtBackendbehind the existingVioBackend+Cov-policy seams — the shipped EKF stays default; the two are A/B-compared on the same EuRoC harness. Do not mutate the shipped filter.Acceptance gate (already built)
Extend
observability_probe.hppto the invariant error and confirm ‖H·N‖_yaw ≈ 0 even at a perturbed estimate (the invariantHis estimate-independent, so the leak the standard EKF showed — 2e-16 → 0.15 — stays at machine-ε for any σ). System gate: EuRoC attitude-NEES → ~1, no divergence. This is the gate the hand-rolled FEJ lacked.Phasing
SE₂(3)invariant error + group propagation + the invariant observability probe (yaw-leak → 0 by construction). If the probe doesn't go flat, stop. → separate spike issue.H); unit + recovery tests.Risk & alternatives
F/Honto𝒩⊥each step — a bounded patch on the current EKF) or the Equivariant Filter (EqF). R-IEKF is the recommended robust answer; OC-EKF is the cheaper consistency patch.Localizes / supersedes the FEJ approach for #212. Refs #337, #339, #212.