# C5 — State Estimator

## 1. High-Level Overview

**Purpose**: own the GTSAM `iSAM2` + `IncrementalFixedLagSmoother` (K=10–20 keyframes per D-C5-3) state. Fuse `VioOutput` (C1), `PoseEstimate` (C4), and FC IMU/attitude windows (C8 inbound) into the posterior pose with native 6×6 covariance via `Marginals` (D-C5-5 = (c)). Emit the smoothed corrected current frame to C8 for FC delivery; emit smoothed past-keyframes to C13 (FDR only — AC-4.5 internal smoothing, NOT FC retroactive correction).

**Architectural Pattern**: Strategy with two concrete implementations: `GtsamIsam2StateEstimator` (production-default) and `EskfStateEstimator` (mandatory simple-baseline). Selection at startup (ADR-001), `BUILD_*` gating (ADR-002), composition-root wired (ADR-009).

**Cycle-1 operational reality**: the airborne binary wires C5 through `_STRATEGY_REGISTRY` + `register_airborne_strategies()` (AZ-591) on top of the `BUILD_STATE_*` build-flag matrix (`runtime_root/airborne_bootstrap.py::C5_STATE_BUILD_FLAGS = {"gtsam_isam2": "BUILD_STATE_GTSAM_ISAM2", "eskf": "BUILD_STATE_ESKF"}`). Both strategies appear in `_C5_STATE_STRATEGIES`; the `gtsam_isam2` flag defaults ON-when-unset and `eskf` defaults OFF-when-unset (mirrors `state_factory._STATE_BUILD_FLAGS`). Strategy registration is lazy: `_ensure_state_strategy_registered` imports the concrete module (`gtsam_isam2_estimator` or `eskf_baseline`) only when the configured strategy's `BUILD_STATE_*` flag is ON, so a binary configured for `eskf` never imports gtsam. Constructor injection flows through the `pre_constructed` dict passed to `compose_root(config, pre_constructed=...)` (AZ-618 umbrella → AZ-623 c5 helpers phase + AZ-625 eager `(estimator, handle)` pair phase). The `c5_state` slot lists `("c5_imu_preintegrator", "c5_se3_utils", "c5_wgs_converter", "c13_fdr")` in `AIRBORNE_REQUIRED_PRE_CONSTRUCTED_KEYS`; `c6_tile_store`, `camera_calibration`, `flight_id`, and `companion_id` are optional (consumed only when `c5_state.orthorectifier.enabled` is True — see AZ-389 / § 7 below). Missing required keys raise `AirborneBootstrapError` at composition time, naming the consumer and missing key. The `c5_imu_preintegrator` is per-process-cached keyed by `config.runtime.camera_calibration_path` (AC-623.2) so two `build_pre_constructed` invocations return the SAME instance — protecting its bias / sample accumulator from a silent reset on re-invocation. AZ-625 short-circuit: `build_pre_constructed` eagerly invokes `build_state_estimator` and stashes the `StateEstimator` under the private `_c5_prebuilt_estimator` key; `_c5_state_wrapper` returns the prebuilt instance so `c4_pose._isam2_handle` and `c5_state._isam2_handle` reference ONE object across the C4 / C5 seam (AC-625.3). AZ-687 replay-mode guard: when `config.mode == "replay"` and the minimal replay `Config` omits the `c5_state` block, the bootstrap skips the eager `(estimator, handle)` build to avoid forcing the gtsam import on the replay binary (the C5 wrapper itself never runs without the block).

**Upstream dependencies**:
- C1 → `VioOutput` (relative pose + IMU bias).
- C4 → `PoseEstimate` (absolute satellite-anchored pose); C4 adds factors directly to C5's iSAM2 graph (shared substrate).
- C8 inbound side → FC `ImuWindow` + `AttitudeWindow` + `GpsHealth` (for warm-start AC-5.1, blackout AC-NEW-8, spoofing-promotion AC-NEW-2 / F7).

**Downstream consumers**:
- C8 outbound side (per-FC encoder) → `EmittedExternalPosition` (5 Hz periodic to FC).
- C6 (mid-flight tile gen via orthorectifier; C5 supplies the `PoseEstimate` + quality_metadata for tile emission).
- C13 FDR (smoothed past-keyframe estimates, source-set switch events, spoofing-rejection events).

## 2. Internal Interfaces

### Interface: `StateEstimator`

| Method | Input | Output | Async | Error Types |
|--------|-------|--------|-------|-------------|
| `set_takeoff_origin` (AZ-490, ADR-010) | `origin: LatLonAlt, sigma_horiz_m: float, sigma_vert_m: float` | `None` | No | `StateEstimatorConfigError`, `EstimatorAlreadyStartedError` |
| `add_vio` | `VioOutput` | `None` | No | `EstimatorDegradedError`, `EstimatorFatalError` |
| `add_pose_anchor` | `PoseEstimate` | `None` | No | `EstimatorDegradedError`, `EstimatorFatalError` |
| `add_fc_imu` | `ImuWindow` | `None` | No | `EstimatorDegradedError` |
| `current_estimate` | `()` | `EstimatorOutput` (smoothed current keyframe) | No | — |
| `smoothed_history` | `n_keyframes: int` | `list[EstimatorOutput]` | No | — |
| `health_snapshot` | `()` | `EstimatorHealth` | No | — |

**Input DTOs**: see C1, C4, C8.

**Output DTOs**:
```
EstimatorOutput:
  frame_id:                       uuid
  position_wgs84:                 LatLonAlt
  orientation_world_T_body:       Quat (w, x, y, z)
  velocity_world:                 Vector3 (m/s)
  covariance_6x6:                 Matrix6
  source_label:                   enum {satellite_anchored, visual_propagated, dead_reckoned}
  last_satellite_anchor_age_ms:   int
  smoothed:                       bool — true for entries from `smoothed_history`
  emitted_at:                     monotonic_ns

EstimatorHealth:
  isam2_state:                   enum {INIT, TRACKING, DEGRADED, LOST}
  keyframe_count:                int
  cov_norm_growing_for_s:        float — AC-NEW-8 monotonicity check
  spoof_promotion_blocked:       bool — AC-NEW-2 / AC-NEW-8 gate state
```

## 3. External API Specification

Not applicable.

## 4. Data Access Patterns

C5 holds the GTSAM iSAM2 state in memory; persistent storage is only via FDR writes (C13 owns the file). No DB queries.

### Storage Estimates

| Table/Collection | Est. Row Count (1yr) | Row Size | Total Size | Growth Rate |
|-----------------|---------------------|----------|------------|-------------|
| In-memory keyframe window | up to 20 keyframes resident | ~2 KB / keyframe (factors + values) | ~40 KB | bounded by IncrementalFixedLagSmoother K=10–20 |

C5 is bounded by design — no unbounded growth.

## 5. Implementation Details

**Algorithmic Complexity**:
- iSAM2 update on factor add: amortised `O(K)` in keyframe count for the typical case; `O(K^2)` worst-case on relinearisation.
- `Marginals.marginalCovariance(pose_key)`: `O(K^3)` in keyframe-window size; the dominant per-frame cost (~30–90 ms steady-state).
- `IncrementalFixedLagSmoother` keeps the active window bounded — older keyframes are marginalised out.

**State Management**:
- iSAM2 graph + Values + Marginals lifecycle for the flight.
- Cold-start ladder (ADR-010, AZ-490): `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` MUST be invoked before any `add_vio` / `add_fc_imu` / `add_pose_anchor` call. The cold-start window closes on the first `add_*` call. iSAM2 attaches a `PriorFactorPose3` at `Pose3.Identity()` (operator origin BECOMES local-ENU (0,0,0)) with diagonal sigmas `[5°, 5°, 5°, sigma_horiz_m, sigma_horiz_m, sigma_vert_m]`; ESKF seeds the nominal position to (0,0,0) and writes the position-block of the error covariance to `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`. The method is **strictly idempotent on identical args** — re-invocation with byte-equal `(origin, sigma_horiz_m, sigma_vert_m)` is a no-op; re-invocation with **different** args raises `StateEstimatorConfigError`. Once the cold-start window closes, further calls raise `EstimatorAlreadyStartedError` (subclass of `StateEstimatorConfigError`). Defaults `default_takeoff_origin_sigma_horiz_m = 5.0`, `default_takeoff_origin_sigma_vert_m = 10.0` live in `C5StateConfig`.
- Source-label state machine: tracks the AC-NEW-2 / AC-NEW-8 spoofing-promotion gate (≥10 s + visual consistency check + ≤ 200 m bounded-delta before re-promoting a previously-spoofed FC GPS source).
- Last-anchor-age timer for AC-1.3 binning.

**Key Dependencies**:

| Library | Version | Purpose |
|---------|---------|---------|
| GTSAM (Python + C++) | per Plan-phase pin | iSAM2 + `CombinedImuFactor` + `BetweenFactorPose3` + `GenericProjectionFactorCal3DS2` + `Marginals` |
| `gtsam_unstable.IncrementalFixedLagSmoother` | per Plan-phase pin | Bounded keyframe window (D-C5-3 K=10–20) |
| Eigen | matches GTSAM | Lie-algebra math |
| OpenCV (`cv2`, AZ-389 orthorectifier subsystem only) | `>=4.11.0.86,<4.12` (cycle-1 relaxed pin; D-CROSS-CVE-1 deferred — see `_docs/_process_leftovers/2026-05-11_d_cross_cve_1_opencv_pin_deferred.md`) | Imported by `_orthorectifier.py` for warp / JPEG encode when `c5_state.orthorectifier.enabled = True`; default OFF means the import is loaded but the cv2 code path is unreached in cycle-1 |

**Error Handling Strategy**:
- `StateEstimatorConfigError`: `set_takeoff_origin` called with a malformed `LatLonAlt` (out of WGS-84 bounds / non-finite) OR with non-positive / non-finite sigmas, OR re-called inside the cold-start window with conflicting args. `EstimatorAlreadyStartedError` (a `StateEstimatorConfigError` subclass): `set_takeoff_origin` called after the first `add_*` call sealed the cold-start window. Caller must surface to operator; takeoff blocked.
- `EstimatorDegradedError`: factor add yielded poor convergence; covariance inflated; emit `EstimatorOutput` with degraded label.
- `EstimatorFatalError`: iSAM2 numerical failure, KEYFRAME_LIMIT exceeded, etc.; emit no `EstimatorOutput` for this tick. AC-5.2 fallback (3 s no estimate → FC IMU-only) applies.
- Spoof-promotion gate (Principle #11 amended, AZ-385 + AZ-490 follow-up): never re-introduce a previously-spoofed FC GPS source until ALL THREE hold — (i) FC `gps_health == STABLE_NON_SPOOFED` for ≥ 10 s, (ii) the next satellite-anchored frame agrees with the FC GPS within a configurable tolerance, AND (iii) the FC's reported position is within ≤ 200 m of the companion's last emitted `PoseEstimate`. The same gate is applied at takeoff when a Manifest `takeoff_origin` is present: an FC GPS reading that disagrees with the operator origin by > 200 m is logged as suspect and the operator origin wins. Document every reject in FDR + GCS STATUSTEXT.

## 6. Extensions and Helpers

| Helper | Purpose | Used By |
|--------|---------|---------|
| `ImuPreintegrator` | shared with C1 | C1, C5 |
| `SE3Utils` | shared with C1, C4 | C1, C4, C5 |
| `WgsConverter` | shared with C4, C8 | C4, C5, C8 |
| `SourceLabelStateMachine` | spoofing-promotion gate logic | C5 only — keep inside the component |

## 7. Caveats & Edge Cases

**Known limitations**:
- AC-4.5 internal smoothing is **onboard only**; the FC log is forward-time only. The smoothed past-keyframe estimates go to FDR, not back to the FC.
- iSAM2 + `IncrementalFixedLagSmoother` requires careful key management; missing keys cause silent factor-add failures — the implementation MUST log every `add_*` call's success/failure status.

**Potential race conditions**:
- Single writer thread for the iSAM2 graph by design. C1 + C4 + C8-inbound deliver to a timestamp-ordered merge queue ahead of C5's writer thread.

**Performance bottlenecks**:
- `Marginals.marginalCovariance(pose_key)` is the per-frame hot spot. D-CROSS-LATENCY-1 hybrid degrades C4's covariance recovery (not C5's) under thermal throttle.

**Cycle-1 Tier-2 follow-up dependencies**:
- AZ-389 **orthorectifier wiring** — `_orthorectifier.py` + `OrthorectifierConfig` (`enabled`, `cov_norm_threshold`, `inlier_floor`, `tile_size_meters`, `tile_size_pixels`, `zoom_level`, `jpeg_quality`) are wired into `C5StateConfig` and `build_state_estimator`. The default is `enabled: bool = False`, which preserves the existing smoke-test wiring that does not provide a `TileStore` — when False the runtime root skips orthorectifier construction entirely. Production enablement is parked pending AZ-624: the airborne `pre_constructed` dict must populate `camera_calibration`, `flight_id`, `companion_id`, and `c6_tile_store` from the operator-supplied manifest / takeoff orchestrator before flipping `orthorectifier.enabled=True`; until AZ-624 lands those four pre-constructed slots, only the test fixture path (`tests/unit/c5_state/test_az389_*.py`) exercises the orthorectifier subsystem.
- AZ-624 **operator-supplied flight metadata** — the `_c5_state_wrapper` and `_build_c5_state_estimator_pair` already accept `flight_id` and `companion_id` kwargs and forward them to `build_state_estimator`. In cycle-1 the airborne `build_pre_constructed` only seeds the `tile_store` slot from `_build_c6_tile_store(config)`; the other three (`camera_calibration`, `flight_id`, `companion_id`) are passed as `None`. Tier-2 follow-up: AZ-624 production `main()` wiring populates these from the manifest + takeoff orchestrator handshake (currently their `None` value means orthorectifier disablement is the only safe runtime state — see prior bullet).

## 8. Dependency Graph

**Must be implemented after**: C1 (input), C4 (input + shared graph), C8 inbound (FC IMU prior).

**Can be implemented in parallel with**: C6, C13 — independent paths.

**Blocks**: C8 outbound (no per-frame estimate), F3 / F4 / F5 / F7 / F9 / F10.

## 9. Logging Strategy

| Log Level | When | Example |
|-----------|------|---------|
| ERROR | `EstimatorFatalError`; iSAM2 numerical failure; AC-5.2 path imminent | `C5 fatal iSAM2 failure; frame=12345; AC-5.2 fallback` |
| WARN | `EstimatorDegradedError`; spoofing-promotion blocked; cov norm growing >2× steady | `C5 degraded: cov_inflation=3.1, spoof_block=true` |
| INFO | Strategy ready; warm-start applied; spoof-promotion gate state changes | `C5 ready: estimator=gtsam_isam2, K=15` |
| DEBUG | per-frame factor adds + smoothed history depth | `C5 frame=12345 vio_added=true pose_added=true imu_added=true smoothed_n=15` |

**Log format**: structured JSON.
**Log storage**: stdout / journald / FDR via C13 (ERROR + WARN always; smoothed past-keyframe entries always go to FDR per AC-4.5; spoofing-promotion-block events go to FDR + GCS STATUSTEXT).