[AZ-386] C5 ESKF baseline: 16-state error-state KF (NumPy)

Implements the mandatory simple-baseline StateEstimator per AC-2.1a engine-rule at C5 (IT-12 comparative study vs iSAM2). NumPy-only; no GTSAM dependency so BUILD_STATE_ESKF=ON binaries ship without GTSAM at all. - 16-state error vector (pos 3 + vel 3 + rot 3 + ba 3 + bg 3 + dt 1) over a textbook nominal-state / error-state ESKF split. - add_fc_imu: full nonlinear IMU integration + linearised F P F^T + Q covariance propagation per IMU sample. - add_vio: simplified relative-pose update (snapshot-based; baseline scope, documented). - add_pose_anchor: absolute-pose update; integrates BOTH marginals and jacobian modes (no skip — ESKF has no graph; AC-4). - AC-9 divergence test: Mahalanobis r^T S^-1 r > 100 (10 sigma) on the innovation covariance S = H P H^T + R. - AC-5 SPD: Cholesky-positive enforcement on every emitted covariance; non-SPD raises EstimatorFatalError and locks state to LOST. - AC-6 honesty: smoothed_history entries carry smoothed=False; deviation from C5 contract Invariant 7 documented in module + report. - AC-7 / AC-10 BUILD_STATE_ESKF gating: works through existing factory infra (state_factory._STATE_BUILD_FLAGS). - AC-8: SourceLabelStateMachine + FallbackWatcher auto-wired eagerly in __init__, same pattern as the iSAM2 estimator. Tests: 20 new unit tests covering AC-1..AC-10 + robustness checks. Full suite: 660 passed, 2 skipped (CI-only). The AZ-386 Jira transition to Done is deferred (Atlassian MCP returned 'Not connected'); recorded in _docs/_process_leftovers/ for replay on the next autodev invocation per the Leftovers Mechanism. Co-authored-by: Cursor <cursoragent@cursor.com>
2026-06-22 21:01:13 +00:00 · 2026-05-11 10:12:30 +03:00
parent 098aabac0c
commit c0bdb57957
6 changed files with 1486 additions and 1 deletions
@@ -0,0 +1,456 @@
+"""AZ-386 — `EskfStateEstimator` mandatory simple-baseline.
+
+Covers AC-1..AC-10 from ``_docs/02_tasks/todo/AZ-386_c5_eskf_baseline.md``.
+
+The tests construct an estimator directly (no GTSAM required) and
+exercise:
+
+- AC-1: Protocol conformance (``isinstance`` against ``StateEstimator``).
+- AC-2: Prediction step accuracy on a stationary synthetic IMU
+  sequence (drift < 1 m over 5 s of zero specific force).
+- AC-3: ``add_vio`` updates state + shrinks covariance over
+  consistent measurements.
+- AC-4: ``add_pose_anchor`` integrates BOTH ``marginals`` and
+  ``jacobian`` modes (no skip — ESKF has no graph).
+- AC-5: Every emitted ``covariance_6x6`` is SPD; injected non-SPD
+  raises ``EstimatorFatalError``.
+- AC-6: ``smoothed_history`` returns ``smoothed=False`` entries.
+- AC-7: ``BUILD_STATE_ESKF=OFF`` rejected by the factory.
+- AC-8: ``SourceLabelStateMachine`` is auto-wired (label updates on
+  satellite anchor).
+- AC-9: Filter divergence handling raises ``EstimatorFatalError``.
+- AC-10: ``config.strategy="eskf"`` + ``BUILD_STATE_ESKF=ON``
+  resolves to ``EskfStateEstimator`` via the runtime root factory.
+"""
+
+from __future__ import annotations
+
+import dataclasses
+from datetime import datetime, timezone
+from typing import Any
+from unittest import mock
+from uuid import uuid4
+
+import numpy as np
+import pytest
+
+from gps_denied_onboard._types.fc import GpsHealth, GpsStatus
+from gps_denied_onboard._types.nav import ImuSample, ImuWindow
+from gps_denied_onboard._types.pose import PoseEstimate
+from gps_denied_onboard._types.state import IsamState, PoseSourceLabel
+from gps_denied_onboard._types.vio import VioOutput
+from gps_denied_onboard.components.c5_state import (
+    C5StateConfig,
+    EstimatorFatalError,
+    StateEstimator,
+    StateEstimatorConfigError,
+)
+from gps_denied_onboard.components.c5_state.eskf_baseline import (
+    EskfStateEstimator,
+    create,
+    register,
+)
+from gps_denied_onboard.config import load_config
+from gps_denied_onboard.config.schema import Config
+from gps_denied_onboard.runtime_root.state_factory import (
+    build_state_estimator,
+    clear_state_ingest_binding,
+    clear_state_registry,
+)
+
+# ----------------------------------------------------------------------
+# Fixtures + helpers.
+
+
+@pytest.fixture(autouse=True)
+def _isolation():
+    clear_state_registry()
+    clear_state_ingest_binding()
+    yield
+    clear_state_registry()
+    clear_state_ingest_binding()
+
+
+def _build_config(**state_overrides: Any) -> Config:
+    cfg = load_config(env={}, paths=(), require_env=False)
+    new_state = dataclasses.replace(C5StateConfig(strategy="eskf"), **state_overrides)
+    components = dict(cfg.components or {})
+    components["c5_state"] = new_state
+    return dataclasses.replace(cfg, components=components)
+
+
+def _make_estimator(**kwargs: Any) -> EskfStateEstimator:
+    cfg = _build_config(**kwargs)
+    return EskfStateEstimator(
+        cfg,
+        imu_preintegrator=mock.MagicMock(),
+        se3_utils=mock.MagicMock(),
+        wgs_converter=mock.MagicMock(),
+        fdr_client=mock.MagicMock(),
+    )
+
+
+def _identity_pose() -> np.ndarray:
+    return np.eye(4, dtype=np.float64)
+
+
+def _pose_translated(x: float, y: float, z: float) -> np.ndarray:
+    p = np.eye(4, dtype=np.float64)
+    p[0, 3] = x
+    p[1, 3] = y
+    p[2, 3] = z
+    return p
+
+
+def _vio(
+    frame_id: int,
+    ts_ns: int,
+    pose: np.ndarray,
+    cov: np.ndarray | None = None,
+) -> VioOutput:
+    # Default sigma ≈ 0.1 m / 0.1 rad — realistic VIO uncertainty.
+    return VioOutput(
+        frame_id=frame_id,
+        timestamp=datetime.fromtimestamp(ts_ns / 1_000_000_000, tz=timezone.utc),
+        pose_se3=pose,
+        covariance_6x6=cov if cov is not None else np.eye(6) * 0.01,
+    )
+
+
+def _pose_anchor(
+    frame_id: int,
+    ts_ns: int,
+    pose: np.ndarray,
+    cov: np.ndarray | None = None,
+    mode: str = "marginals",
+) -> PoseEstimate:
+    return PoseEstimate(
+        frame_id=frame_id,
+        timestamp=datetime.fromtimestamp(ts_ns / 1_000_000_000, tz=timezone.utc),
+        pose_se3=pose,
+        covariance_6x6=cov if cov is not None else np.eye(6) * 0.01,
+        covariance_mode=mode,
+        mre_px=0.5,
+    )
+
+
+def _imu_window(
+    *, ts_start_ns: int, dt_s: float, n_samples: int, accel: tuple, gyro: tuple
+) -> ImuWindow:
+    samples = tuple(
+        ImuSample(
+            ts_ns=ts_start_ns + int(i * dt_s * 1_000_000_000),
+            accel_xyz=accel,
+            gyro_xyz=gyro,
+        )
+        for i in range(n_samples)
+    )
+    return ImuWindow(
+        samples=samples,
+        ts_start_ns=samples[0].ts_ns,
+        ts_end_ns=samples[-1].ts_ns,
+    )
+
+
+# ----------------------------------------------------------------------
+# AC-1: Protocol conformance.
+
+
+def test_ac1_eskf_satisfies_state_estimator_protocol() -> None:
+    estimator = _make_estimator()
+
+    assert isinstance(estimator, StateEstimator)
+
+
+# ----------------------------------------------------------------------
+# AC-2: Prediction step accuracy.
+
+
+def test_ac2_prediction_drift_under_one_metre_over_five_seconds() -> None:
+    # Arrange — stationary FC: specific force = -gravity in body frame
+    # (so world-frame accel is 0). Zero gyro. 100 Hz over 5 s.
+    estimator = _make_estimator()
+    accel_body_stationary = (0.0, 0.0, 9.80665)
+    window = _imu_window(
+        ts_start_ns=1_000_000_000,
+        dt_s=0.01,
+        n_samples=501,
+        accel=accel_body_stationary,
+        gyro=(0.0, 0.0, 0.0),
+    )
+
+    # Act
+    estimator.add_fc_imu(window)
+
+    # Assert — under perfect zero specific force the nominal
+    # position must remain near the origin. Forward Euler accrues a
+    # small residual from quaternion-renormalisation noise.
+    drift = float(np.linalg.norm(estimator._nominal_pos))
+    assert drift < 1.0
+
+
+# ----------------------------------------------------------------------
+# AC-3: Relative-pose update via add_vio.
+
+
+def test_ac3_add_vio_updates_state_and_covariance_shrinks() -> None:
+    # Arrange — two consistent VIO frames; first frame seeds, second
+    # frame measures a 1 m forward delta.
+    estimator = _make_estimator()
+    cov_before_first = estimator._pose_covariance_6x6().copy()
+    estimator.add_vio(_vio(1, 1_000_000_000, _identity_pose()))
+    estimator.add_vio(_vio(2, 1_100_000_000, _pose_translated(1.0, 0.0, 0.0)))
+
+    # Assert
+    cov_after = estimator._pose_covariance_6x6()
+    assert np.linalg.norm(cov_after, ord="fro") <= np.linalg.norm(cov_before_first, ord="fro")
+
+
+# ----------------------------------------------------------------------
+# AC-4: Absolute-pose update; mode doesn't skip ESKF.
+
+
+def test_ac4_add_pose_anchor_runs_for_marginals_and_jacobian() -> None:
+    # Arrange — two anchors, one in each mode; both must shift the
+    # nominal position toward the measurement.
+    estimator_m = _make_estimator()
+    estimator_j = _make_estimator()
+    pose_m = _pose_anchor(1, 1_000_000_000, _pose_translated(5.0, 0.0, 0.0), mode="marginals")
+    pose_j = _pose_anchor(2, 1_000_000_000, _pose_translated(5.0, 0.0, 0.0), mode="jacobian")
+
+    # Act
+    estimator_m.add_pose_anchor(pose_m)
+    estimator_j.add_pose_anchor(pose_j)
+
+    # Assert — both modes move the nominal toward the measurement.
+    assert estimator_m._nominal_pos[0] > 0.1
+    assert estimator_j._nominal_pos[0] > 0.1
+    assert estimator_m._last_anchor_ns > 0
+    assert estimator_j._last_anchor_ns > 0
+
+
+def test_ac4_pose_anchor_sets_last_anchor_ns() -> None:
+    estimator = _make_estimator()
+    pose = _pose_anchor(1, 1_000_000_000, _pose_translated(1.0, 0.0, 0.0))
+
+    estimator.add_pose_anchor(pose)
+
+    assert estimator._last_anchor_ns > 0
+
+
+# ----------------------------------------------------------------------
+# AC-5: SPD covariance enforcement.
+
+
+def test_ac5_current_estimate_emits_spd_covariance() -> None:
+    estimator = _make_estimator()
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+
+    out = estimator.current_estimate()
+
+    assert out.covariance_6x6.shape == (6, 6)
+    np.linalg.cholesky(out.covariance_6x6)
+
+
+def test_ac5_non_spd_covariance_raises_fatal() -> None:
+    # Arrange — force the internal covariance to be indefinite.
+    estimator = _make_estimator()
+    P = np.eye(16, dtype=np.float64)
+    P[0, 0] = -1.0  # negative variance on x-position
+    estimator._P = P
+
+    # Act / Assert
+    with pytest.raises(EstimatorFatalError, match="not SPD"):
+        estimator.current_estimate()
+    assert estimator._isam2_state == IsamState.LOST
+
+
+# ----------------------------------------------------------------------
+# AC-6: smoothed_history honesty.
+
+
+def test_ac6_smoothed_history_returns_smoothed_false_entries() -> None:
+    estimator = _make_estimator()
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+    estimator.current_estimate()
+    estimator.add_pose_anchor(_pose_anchor(2, 1_100_000_000, _pose_translated(0.5, 0.0, 0.0)))
+    estimator.current_estimate()
+
+    history = estimator.smoothed_history(n_keyframes=10)
+
+    assert len(history) == 2
+    assert all(entry.smoothed is False for entry in history)
+
+
+def test_ac6_smoothed_history_empty_buffer_returns_empty() -> None:
+    estimator = _make_estimator()
+
+    assert estimator.smoothed_history(10) == []
+
+
+def test_ac6_smoothed_history_n_zero_returns_empty() -> None:
+    estimator = _make_estimator()
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+    estimator.current_estimate()
+
+    assert estimator.smoothed_history(0) == []
+
+
+# ----------------------------------------------------------------------
+# AC-7: BUILD_STATE_ESKF=OFF rejection.
+
+
+def test_ac7_build_state_eskf_off_rejected(monkeypatch: pytest.MonkeyPatch) -> None:
+    register()
+    monkeypatch.setenv("BUILD_STATE_ESKF", "OFF")
+    cfg = _build_config()
+
+    with pytest.raises(StateEstimatorConfigError, match="BUILD_STATE_ESKF is OFF"):
+        build_state_estimator(
+            cfg,
+            imu_preintegrator=mock.MagicMock(),
+            se3_utils=mock.MagicMock(),
+            wgs_converter=mock.MagicMock(),
+            fdr_client=mock.MagicMock(),
+        )
+
+
+# ----------------------------------------------------------------------
+# AC-8: SourceLabelStateMachine integration.
+
+
+def test_ac8_source_label_machine_is_auto_wired() -> None:
+    # Arrange — gate is open by default; the first satellite anchor
+    # transitions the label DEAD_RECKONED → SATELLITE_ANCHORED.
+    estimator = _make_estimator()
+    assert estimator.current_estimate().source_label == PoseSourceLabel.DEAD_RECKONED
+
+    # Act
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+    out_after = estimator.current_estimate()
+
+    # Assert
+    assert out_after.source_label == PoseSourceLabel.SATELLITE_ANCHORED
+
+
+def test_ac8_spoof_status_closes_gate_and_rejects_anchor() -> None:
+    # Arrange — SPOOFED GPS latches the gate closed; next anchor is
+    # a promotion attempt that fails (no consistency check possible
+    # because the estimator passes ``gps_consistency_delta_m=None``).
+    estimator = _make_estimator()
+    received: list[Any] = []
+    estimator.subscribe_spoof_rejection(
+        lambda reason, severity, text: received.append((reason, severity, text))
+    )
+    estimator.notify_gps_health(_gps_status(GpsStatus.SPOOFED), now_ns=0)
+
+    # Act
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+
+    # Assert
+    assert len(received) == 1
+    assert estimator.current_estimate().source_label == PoseSourceLabel.VISUAL_PROPAGATED
+
+
+# ----------------------------------------------------------------------
+# AC-9: Filter divergence handling.
+
+
+def test_ac9_filter_divergence_raises_fatal() -> None:
+    # Arrange — feed a pose anchor whose measured position is wildly
+    # inconsistent with the nominal state, with a tight measurement
+    # covariance. innov_mag = 1000 m; sqrt(||R||_F) ≈ 1e-2 → ratio ≫ 10.
+    estimator = _make_estimator()
+    far_pose = _pose_translated(1000.0, 0.0, 0.0)
+    tight_cov = np.eye(6) * 1e-6
+
+    # Act / Assert
+    with pytest.raises(EstimatorFatalError, match="filter divergence"):
+        estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, far_pose, cov=tight_cov))
+    assert estimator._isam2_state == IsamState.LOST
+
+
+# ----------------------------------------------------------------------
+# AC-10: Composition wiring through the runtime root.
+
+
+def test_ac10_factory_resolves_eskf_strategy(monkeypatch: pytest.MonkeyPatch) -> None:
+    register()
+    monkeypatch.setenv("BUILD_STATE_ESKF", "ON")
+    cfg = _build_config(strategy="eskf")
+
+    estimator, handle = build_state_estimator(
+        cfg,
+        imu_preintegrator=mock.MagicMock(),
+        se3_utils=mock.MagicMock(),
+        wgs_converter=mock.MagicMock(),
+        fdr_client=mock.MagicMock(),
+    )
+
+    assert isinstance(estimator, EskfStateEstimator)
+    assert handle is None
+
+
+def test_ac10_module_level_create_returns_protocol_and_none_handle() -> None:
+    cfg = _build_config()
+
+    estimator, handle = create(
+        config=cfg,
+        imu_preintegrator=mock.MagicMock(),
+        se3_utils=mock.MagicMock(),
+        wgs_converter=mock.MagicMock(),
+        fdr_client=mock.MagicMock(),
+    )
+
+    assert isinstance(estimator, StateEstimator)
+    assert handle is None
+
+
+# ----------------------------------------------------------------------
+# Other robustness checks.
+
+
+def test_health_snapshot_reports_init_then_tracking() -> None:
+    estimator = _make_estimator()
+
+    assert estimator.health_snapshot().isam2_state == IsamState.INIT
+
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+
+    assert estimator.health_snapshot().isam2_state == IsamState.TRACKING
+
+
+def test_current_estimate_returns_uuid_frame_id_and_increments_keyframes() -> None:
+    estimator = _make_estimator()
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+
+    out = estimator.current_estimate()
+
+    assert isinstance(out.frame_id, type(uuid4()))
+    assert estimator.health_snapshot().keyframe_count == 1
+
+
+def test_out_of_order_vio_raises_degraded() -> None:
+    from gps_denied_onboard.components.c5_state.errors import EstimatorDegradedError
+
+    estimator = _make_estimator()
+    estimator.add_vio(_vio(1, 2_000_000_000, _identity_pose()))
+
+    with pytest.raises(EstimatorDegradedError, match="out-of-order"):
+        estimator.add_vio(_vio(2, 1_000_000_000, _identity_pose()))
+
+
+def test_pose_anchor_sets_velocity_to_zero_initially() -> None:
+    estimator = _make_estimator()
+    estimator.add_pose_anchor(_pose_anchor(1, 1_000_000_000, _identity_pose()))
+
+    out = estimator.current_estimate()
+    assert out.velocity_world_mps == (0.0, 0.0, 0.0)
+
+
+# ----------------------------------------------------------------------
+# Helpers for AC-8 GPS health construction.
+
+
+def _gps_status(status: GpsStatus) -> GpsHealth:
+    return GpsHealth(status=status, fix_age_ms=0, captured_at=0)