Skip to content

feat(sdk): online extrinsic calibration (S10-as-state) for the #212 over-confidence #332

Description

@Ravenwater

Problem

The EuRoC filter is accurate but over-confident (#212): NEES ≈ 140 vs ~15 expected on V2_03. This session localized the cause by measurement to the input side — the EKF algebra is clean (S6 NIS/dof = 1.00), and two blunt remedies were refuted end-to-end: the S5 parallax gate (#325) and a global/isotropic calibration R-term (#328). The R-term confirmed the diagnosis (V2_03 NIS 14.7 → 1.41) but regressed ATE/NEES — a uniform R-inflation just down-weights all vision and starves the fast-motion filter into IMU dead-reckoning.

The principled fix the S10 verdict points at: estimate the camera↔IMU calibration online so the calibration error is corrected, not the signal drowned. See docs/arch/vio-pipeline-canonical.md §S10 and the Engineering Status decomposition.

Goal & decisive test

Promote T_CI from a fixed config value to estimated state.

Success criterion (go/no-go): on EuRoC V2_03 + MH_05 with estimation on, NEES drops toward dim(state) AND ATE does not regress — the opposite of the refuted R-inflation (NIS↓ but ATE/NEES↑). Calibration-state NEES vs GT extrinsics stays consistent, and a deliberately-perturbed T_CI is estimated back to truth.

Scope — Tier 1 only (extrinsics)

In: T_CI = (R_ic ∈ SO(3), p_ic ∈ ℝ³), 6 error-states/cam. Toggle VioConfig::estimate_extrinsics (default false → zero behavior change when off).

Deferred (follow-ups): time offset t_d (needs image-velocity Jacobian, MINS-style), intrinsics/distortion (EuRoC intrinsics are well-calibrated), IMU intrinsics.

Rationale: the S10 budget showed extrinsic rotation (≈8 px @ 1°) dominates the calibration error.

Implementation plan (file by file)

  1. msckf/state.hpp — layout (highest-risk). Insert a fixed calib block between IMU and clones: [IMU 15][calib 6·N][clone 6]…. Carry the calib mean + calib_offset(); route dim()/clone_offset() through a runtime calib_dim(); size the initial covariance to kImuDim + calib_dim.
  2. Propagation (propagator.hpp + covariance policy). Calib states are static (identity Φ, zero Q) — like clones. ⚠️ verify the policy applies IMU Φ to [0,15) and identity to ≥15.
  3. state_helper.hpp — augment/marginalize. Nearly free: augment_clone's G is identity over existing states (calib rides along); marginalize_clone keeps calib. The existing S3/S9 probes are the regression guard.
  4. camera_updater.hpp — Jacobians. Read the extrinsic from state when on; add (analytic, finite-diff checked): ∂h/∂δθ_ic = dh·[p_c]_×, ∂h/∂δp_ic = −dh·R_ct, filling the calib columns of H_x for every observation (the shared coupling that makes it observable). Null-space projection just sees a wider H_x.
  5. state_helper.hpp::ekf_update — mean box-plus for R_ic (SO3) / p_ic, mirroring the clone loop.
  6. msckf_backend.hpp — init. Seed the calib P₀ block with the prior (S10 budget: calib_ext_rot_prior_deg ≈ 1°, calib_ext_trans_prior_mm, configurable). Off ⇒ no block ⇒ current behavior bit-for-bit.

Test / probe plan

  • Unit: ∂h/∂ext analytic-vs-finite-diff; augment/marg preserve calib block & PSD; recover-injected-extrinsic (perturb T_CI 2°, filter estimates it back).
  • Probe: extend eval/calibration_probe.hpp with an estimation path → calibration-state NEES vs GT + pose-NEES restoration on the synthetic world (drives the real backend).
  • Decisive EuRoC gate: V2_03 + MH_05 with the flag on → NEES/NIS/ATE vs baseline.

PR breakdown

  • PR-1 — mechanism + synthetic proof: state layout, plumbing, Jacobians, estimate_extrinsics flag, probe + unit tests. Ships default-off, proven on the synthetic world (recovers injected T_CI, restores synthetic NEES).
  • PR-2 — EuRoC verdict: the real V2_03/MH_05 sweep, canonical §S10 + engineering-status update, default on/off decision based on the measurement.

Risks (named)

  • Observability collapse on low excitation → calib drifts and worsens. Mitigate: tight prior + optional excitation gate + (if needed) FEJ on the calib states.
  • t_d co-contributor (EuRoC ~ms offset) → extrinsics may only partially close NEES; PR-2 quantifies the residual (→ Tier 2).
  • Layout refactor touches covariance sizing/offsets — guarded by the "inert when off" invariant + the S3/S9 probes.

Sub-task of #212 (over-confidence umbrella); instrument provenance #261. The diagnostic chain that motivates this fix is in docs/arch/vio-pipeline-canonical.md §S10.

Metadata

Metadata

Assignees

Labels

enhancementNew feature or requestphase-3-vioVisual-Inertial Odometry

Type

No type

Projects

No projects

Relationships

None yet

Development

No branches or pull requests

Issue actions