# Contract: `StateEstimator` Protocol

**Owner**: c5_state (epic AZ-260 / E-C5)
**Producer task**: AZ-381 (Protocol + DTOs + factory + composition + concrete `ISam2GraphHandle`)
**Consumer tasks**: AZ-382 (iSAM2 + IncrementalFixedLagSmoother wiring), AZ-383 (Factor adds), AZ-384 (Marginals + outputs), AZ-385 (Source-label + spoof gate), AZ-386 (ESKF baseline), AZ-387 (Smoothed history → FDR), AZ-388 (AC-5.2 fallback), AZ-389 (Orthorectifier → C6), AZ-490 (set_takeoff_origin — operator-provided warm-start).
**Version**: 1.1.0
**Status**: active
**Last Updated**: 2026-05-11
**Module-layout home**: `src/gps_denied_onboard/components/c5_state/interface.py`, `src/gps_denied_onboard/components/c5_state/__init__.py`, `src/gps_denied_onboard/runtime_root/state_factory.py`

## Purpose

Defines the public interface for the C5 state estimator: fuses `VioOutput` (C1), `PoseEstimate` (C4), and FC `ImuWindow` (C8 inbound) into the posterior pose with native 6×6 covariance. Two concrete strategies linked at build time per ADR-002: `GtsamIsam2StateEstimator` (production-default; iSAM2 + IncrementalFixedLagSmoother K=10–20 per D-C5-3) and `EskfStateEstimator` (mandatory simple-baseline per IT-12 engine rule). Selected at startup via `config.state.strategy` with `BUILD_STATE_<variant>` flag gating per ADR-002.

C5 owns the GTSAM iSAM2 graph (ADR-003 shared substrate); C4's `OpenCVGtsamPoseEstimator` adds factors to this graph via the `ISam2GraphHandle` Protocol (defined by AZ-355 stub; concrete impl owned by AZ-381 — first child of E-C5). Single-writer thread invariant: composition root binds C5 to the same ingest thread as C4.

The shared `ImuPreintegrator` (AZ-276), `SE3Utils` (AZ-277), and `WgsConverter` (AZ-279) helpers are constructor-injected.

## Public API

### Protocol: `StateEstimator`

```python
@runtime_checkable
class StateEstimator(Protocol):
    # AZ-490 / ADR-010: operator-provided warm-start. MUST be called before any add_*.
    def set_takeoff_origin(
        self,
        origin: LatLonAlt,
        sigma_horiz_m: float,
        sigma_vert_m: float,
    ) -> None: ...

    def add_vio(self, vio: VioOutput) -> None: ...
    def add_pose_anchor(self, pose: PoseEstimate) -> None: ...
    def add_fc_imu(self, imu_window: ImuWindow) -> None: ...
    def current_estimate(self) -> EstimatorOutput: ...
    def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]: ...
    def health_snapshot(self) -> EstimatorHealth: ...
```

**Invariants**:

1. **Single-writer thread** — every `add_*` and `current_estimate`/`smoothed_history` runs on the same ingest thread; ADR-003 GTSAM substrate is non-thread-safe.
2. **`add_*` calls are timestamp-ordered** — composition root provides a merge queue; out-of-order arrivals are rejected with `EstimatorDegradedError`.
3. **`add_pose_anchor(pose)` MUST inspect `pose.covariance_mode`** — `JACOBIAN` mode adds the pose to the running estimate but DOES NOT add an iSAM2 factor (per AZ-361 cross-task interaction); `MARGINALS` mode triggers the full factor add + iSAM2 update.
4. **`current_estimate()` ALWAYS returns a fresh `EstimatorOutput`** — never None on the steady-state path; `EstimatorFatalError` propagates if iSAM2 is unrecoverable.
5. **`source_label` reflects gate state** — `SATELLITE_ANCHORED` only when the spoof-promotion gate confirms (≥10 s `STABLE_NON_SPOOFED` AND visual-consistent next anchor); else `VISUAL_PROPAGATED` or `DEAD_RECKONED`.
6. **`smoothed_history(n)` returns up to K keyframes** — K bounded by `IncrementalFixedLagSmoother` window (D-C5-3 K=10–20); out-of-window keyframes are NOT recoverable.
7. **`smoothed_history(n)` entries have `smoothed=True`** — distinguishes from `current_estimate()` which has `smoothed=False`.
8. **Spoof-rejection events ALWAYS land in FDR + GCS STATUSTEXT** — never silent (R07; C5-ST-01).
9. **AC-5.2 fallback on 3 s no-estimate** — if `current_estimate()` would raise OR the keyframe window is empty for ≥3 s, downstream C8 emits FC IMU-only.
10. **`covariance_6x6` is always SPD** — both strategies enforce; on numerical failure raise `EstimatorFatalError`.
11. **`set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` is a cold-start-window-only entrypoint** (AZ-490, ADR-010). The cold-start window closes on the first `add_*` call; calling `set_takeoff_origin` after that raises `EstimatorAlreadyStartedError` (subclass of `StateEstimatorConfigError`). Inside the cold-start window the call is **strictly idempotent on identical args** (re-invocation with byte-equal `origin`/`sigma_horiz_m`/`sigma_vert_m` is a no-op); a re-invocation with **different** args raises `StateEstimatorConfigError`. Both sigmas MUST be positive and finite; the origin MUST be inside WGS-84 bounds (lat ∈ [-90, 90], lon ∈ [-180, 180], all components finite); otherwise raise `StateEstimatorConfigError`. The origin is consumed as a Bayesian prior on the first pose key (iSAM2: `PriorFactorPose3` at `Pose3.Identity()` with diagonal sigmas `[5°, 5°, 5°, sigma_horiz_m, sigma_horiz_m, sigma_vert_m]` in rotation+translation order; the operator origin BECOMES the local-ENU (0,0,0) anchor. ESKF: nominal-state position seeded to (0,0,0) + position block of error covariance set to `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`). One `c5.cold_start_origin.set` FDR record is emitted per estimator (with `source="manifest"` from this entrypoint, or `source="fc_ekf"` from the legacy first-frame path when no operator origin was supplied).
12. **Spoof-promotion gate has THREE clauses, not two** — Principle #11 amended. Re-promote a previously-spoofed FC GPS source only when ALL of: (i) FC `gps_health == STABLE_NON_SPOOFED` for ≥ `spoof_promotion_min_stable_s`, (ii) the next satellite-anchored frame agrees with the FC GPS within `spoof_promotion_visual_consistency_tol_m`, AND (iii) the FC's reported position is within `spoof_promotion_bounded_delta_m` (default 200 m) of the companion's last emitted `PoseEstimate`. The bounded-delta clause also gates the takeoff path when a Manifest `takeoff_origin` is present.

### DTOs (in `_types/state.py`)

```python
@dataclass(frozen=True, slots=True)
class EstimatorOutput:
    frame_id: UUID
    position_wgs84: LatLonAlt
    orientation_world_T_body: Quat
    velocity_world_mps: tuple[float, float, float]
    covariance_6x6: np.ndarray
    source_label: PoseSourceLabel
    last_satellite_anchor_age_ms: int
    smoothed: bool
    emitted_at: int


class IsamState(Enum):
    INIT = "init"
    TRACKING = "tracking"
    DEGRADED = "degraded"
    LOST = "lost"


@dataclass(frozen=True, slots=True)
class EstimatorHealth:
    isam2_state: IsamState
    keyframe_count: int
    cov_norm_growing_for_s: float
    spoof_promotion_blocked: bool
```

### Error hierarchy (in `c5_state/errors.py`)

```python
class StateEstimatorError(Exception): pass
class EstimatorDegradedError(StateEstimatorError): pass  # poor convergence; emit degraded estimate
class EstimatorFatalError(StateEstimatorError): pass    # numerical failure; AC-5.2 path
class StateEstimatorConfigError(StateEstimatorError): pass  # composition-time
```

### Composition-root factory

```python
def build_state_estimator(
    config: AppConfig,
    imu_preintegrator: ImuPreintegrator,
    se3_utils: SE3Utils,
    wgs_converter: WgsConverter,
    fdr_client: FdrClient,
) -> tuple[StateEstimator, ISam2GraphHandle]:
    """Construct the configured state estimator + return the iSAM2 graph handle for C4 to inject. Selects between gtsam_isam2 / eskf via config; ADR-002 BUILD_STATE_<variant> gating."""
    ...
```

Strategy resolution table:

| `config.state.strategy` | Module path | Class |
|---|---|---|
| `"gtsam_isam2"` | `gps_denied_onboard.components.c5_state.gtsam_isam2_estimator` | `GtsamIsam2StateEstimator` |
| `"eskf"` | `gps_denied_onboard.components.c5_state.eskf_baseline` | `EskfStateEstimator` |

Config schema additions:

- `config.state.strategy` (enum; required)
- `config.state.keyframe_window_size` (int, default 15) — D-C5-3 K=10–20
- `config.state.spoof_promotion_min_stable_s` (float, default 10.0) — AC-NEW-2
- `config.state.spoof_promotion_visual_consistency_tol_m` (float, default 30.0) — AC-NEW-8
- `config.state.spoof_promotion_bounded_delta_m` (float, default 200.0) — Principle #11 amended, ADR-010
- `config.state.no_estimate_fallback_s` (float, default 3.0) — AC-5.2
- `config.state.default_takeoff_origin_sigma_horiz_m` (float, default 50.0) — AZ-490 default horizontal sigma when caller omits
- `config.state.default_takeoff_origin_sigma_vert_m` (float, default 100.0) — AZ-490 default vertical sigma when caller omits

## Test expectations summarised by Invariant

| Invariant | Test | Assertion |
|---|---|---|
| 1 | Thread-binding | second binding from a different thread → `RuntimeError` |
| 2 | Timestamp ordering | out-of-order `add_*` → `EstimatorDegradedError` |
| 3 | `add_pose_anchor` mode dispatch | JACOBIAN: no iSAM2 factor add; MARGINALS: factor + update |
| 4 | `current_estimate` shape | always returns fresh `EstimatorOutput` on steady state |
| 5 | Spoof gate | label reflects gate state |
| 6 | Smoothed history bounded | `len(smoothed_history(100))` ≤ K |
| 7 | Smoothed flag | every `smoothed_history` entry has `smoothed=True`; `current_estimate` has `smoothed=False` |
| 8 | Spoof-rejection logging | FDR + GCS STATUSTEXT both fire on every gate decision |
| 9 | AC-5.2 timeout | 3 s no estimate → fallback signal emitted |
| 10 | SPD covariance | every emitted `covariance_6x6` is SPD |
| 11a | `set_takeoff_origin` after first `add_*` call | raises `EstimatorAlreadyStartedError` (subclass of `StateEstimatorConfigError`) |
| 11b | `set_takeoff_origin` with non-positive / non-finite sigma OR out-of-bounds `LatLonAlt` | raises `StateEstimatorConfigError` |
| 11c | `set_takeoff_origin` twice in cold-start window with **identical** args | second call is a no-op; no extra FDR record |
| 11d | `set_takeoff_origin` twice in cold-start window with **different** args | raises `StateEstimatorConfigError` (names both prev/new args) |
| 11e | First `current_estimate` after `set_takeoff_origin` + no sensor samples | returns `EstimatorOutput` with `position_wgs84 == origin`, position block of `covariance_6x6` reflecting `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)` |
| 12 | Bounded-delta gate | FC GPS frame with |Δ| > 200 m vs last emitted `PoseEstimate` is rejected even when stable + non-spoofed for ≥ 10 s + visual-consistent |

## Producer-task / consumer-task split

1. **Protocol + composition + ISam2GraphHandle concrete** (Producer; 3 pts): Protocol, DTOs, error hierarchy, factory, config schema, Concrete `ISam2GraphHandle` impl extending the AZ-355 stub.
2. **iSAM2 + IncrementalFixedLagSmoother K wiring** (5 pts): GTSAM graph construction, K=10–20 window, key management.
3. **Factor adds (VIO + Pose + IMU)** (5 pts): `BetweenFactorPose3`, `GenericProjectionFactorCal3DS2`, `CombinedImuFactor` per the input DTO.
4. **Marginals + outputs** (3 pts): `current_estimate` / `smoothed_history` / `health_snapshot` body using `Marginals`.
5. **Source-label + spoof-promotion gate** (5 pts): `SourceLabelStateMachine` + AC-NEW-2 / AC-NEW-8 logic.
6. **ESKF baseline** (5 pts): `EskfStateEstimator` mandatory simple-baseline (IT-12 engine rule).
7. **Smoothed history → FDR** (3 pts): writer path + AC-4.5 invariant (NOT to FC).
8. **AC-5.2 fallback** (3 pts): 3 s no-estimate detector + signal emission.
9. **Orthorectifier → C6 mid-flight tile** (3 pts): orthorectifier sub-path.

Total: 35 pts (within the XL 34–55 band).
