# 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).

**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 |
|--------|-------|--------|-------|-------------|
| `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.
- Source-label state machine: tracks the AC-NEW-2 / AC-NEW-8 spoofing-promotion gate (≥10 s + visual consistency check 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 |

**Error Handling Strategy**:
- `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: never re-introduce a previously-spoofed FC GPS source until BOTH (i) FC `gps_health == STABLE_NON_SPOOFED` for ≥ 10 s AND (ii) the next satellite-anchored frame agrees with the FC GPS within a configurable tolerance (AC-NEW-8). 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.

## 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).