# C5 GtsamIsam2StateEstimator — iSAM2 + IncrementalFixedLagSmoother K=10–20 wiring

**Task**: AZ-382_c5_isam2_smoother_wiring
**Name**: C5 `GtsamIsam2StateEstimator` skeleton — iSAM2 + IncrementalFixedLagSmoother (K=10–20) wiring
**Description**: Implement the `GtsamIsam2StateEstimator` class skeleton with the GTSAM iSAM2 + `gtsam_unstable.IncrementalFixedLagSmoother` lifecycle: graph + `Values` containers; key-management policy (`gtsam.symbol('x', frame_id_int)` for poses, `'b'` for bias, `'v'` for velocity); window size K from `config.state.keyframe_window_size` (default 15; D-C5-3 K=10–20). REPLACE the `NotImplementedError` skeleton bodies in `_isam2_handle.py` (from AZ-381) with the actual `add_factor`/`update`/`compute_marginals`/`last_anchor_age_ms` implementations against this estimator's iSAM2 graph. The estimator's `add_vio`/`add_pose_anchor`/`add_fc_imu` methods still raise `NotImplementedError("Factor adds owned by AZ-383")` until the next task lands. `current_estimate`/`smoothed_history`/`health_snapshot` similarly. This task delivers the foundational graph + handle wiring on which all subsequent C5 tasks depend.
**Complexity**: 5 points
**Dependencies**: AZ-381 (Protocol + factory + handle skeleton), AZ-263, AZ-269, AZ-266, AZ-272 (FDR record schema)
**Component**: c5_state (epic AZ-260 / E-C5)
**Tracker**: AZ-382
**Epic**: AZ-260 (E-C5)

### Document Dependencies

- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — Protocol surface.
- `_docs/02_document/components/07_c5_state/description.md` — § 5 (iSAM2 + IncrementalFixedLagSmoother dependencies); § 7 (single-writer thread; missing-key silent failure mitigation).
- `_docs/02_document/architecture.md` — ADR-003 (shared GTSAM substrate); D-C5-3 (K=10–20 keyframe window).

## Problem

Without this task, the GTSAM iSAM2 graph does not exist; C4's `add_factor` calls have no target; `Marginals` cannot be computed; the `ISam2GraphHandleImpl` skeleton's `NotImplementedError` bodies would block all C4 wiring. Every subsequent C5 task (factor adds, marginals, source-label, smoothed-history) needs the graph + handle to be real.

## Outcome

- `src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py` defining:
  - `GtsamIsam2StateEstimator` class implementing `StateEstimator`.
  - Constructor: `__init__(self, config, imu_preintegrator, se3_utils, wgs_converter, fdr_client)`.
  - Internal: `self._isam2 = gtsam.ISAM2(parameters)`, `self._smoother = gtsam.IncrementalFixedLagSmoother(K * frame_period_s)`, `self._graph = gtsam.NonlinearFactorGraph()`, `self._values = gtsam.Values()`, `self._key_for_frame: dict[UUID, int]`.
  - Module-level `create(config, imu_preintegrator, se3_utils, wgs_converter, fdr_client) -> StateEstimator` factory function.
  - `add_*` methods raise `NotImplementedError("Factor adds owned by AZ-383")` (next task).
  - `current_estimate`/`smoothed_history`/`health_snapshot` raise `NotImplementedError("Marginals + outputs owned by AZ-384")`.
- Replace bodies of `ISam2GraphHandleImpl` in `_isam2_handle.py` (from AZ-381) — REAL `add_factor` (calls `self._graph.add(factor)`), REAL `update` (calls `self._isam2.update(graph, values)` + `self._smoother.update(graph, values, timestamps)`), REAL `compute_marginals` (returns `gtsam.Marginals(graph, values)`), REAL `last_anchor_age_ms` (tracks `last_anchor_monotonic_ns` updated by AZ-383 when a satellite-anchored pose is added).
- DEBUG log on construction: `kind="c5.state.isam2_initialised"` with `{keyframe_window_size, total_factors_initial: 0}`.
- Defensive logging: every internal mutation of `_isam2`/`_smoother` MUST be wrapped in success/failure logging per § 7 (R05 mitigation: "every add logs success/false").

## Scope

### Included
- `GtsamIsam2StateEstimator` class scaffold.
- iSAM2 + `IncrementalFixedLagSmoother` initialisation; key-management policy.
- `ISam2GraphHandleImpl` body replacement (the four real methods).
- Defensive success/failure logging for every iSAM2 / smoother mutation (R05 mitigation).
- Config schema check: `keyframe_window_size` in [10, 20].
- Composition-root invocation path: factory `create(...)` returns the estimator; `state_factory` extracts the `_isam2_handle` reference.
- Unit tests: graph + values construction; handle method bodies reachable; key-management policy assigns unique keys per frame; defensive logging emits on every mutation.

### Excluded
- Factor adds (VIO/Pose/IMU) — owned by next task.
- Marginals + outputs — owned by next task.
- Source-label + spoof gate — owned by next task.
- AC-5.2 fallback — owned by next task.
- ESKF — owned by AZ-386.
- Smoothed history → FDR path body (the FDR write itself is the smoothed-history task; this task only ensures `_smoother` is constructed).
- C5-IT/PT/ST tests — deferred to E-BBT.

## Acceptance Criteria

**AC-1: Construction** — `GtsamIsam2StateEstimator(...)` instantiates without error; `_isam2`, `_smoother`, `_graph`, `_values`, `_key_for_frame` are all initialised.

**AC-2: Key-management policy** — frame-IDs map to unique GTSAM keys via `gtsam.symbol('x', counter)`; `_key_for_frame` is consulted before assigning new keys.

**AC-3: Window size respected** — `IncrementalFixedLagSmoother` instantiated with `K * frame_period_s` (e.g., K=15 × 0.333 s = 5 s window).

**AC-4: Window size validation** — `keyframe_window_size = 5` (out of [10, 20] range) → `StateEstimatorConfigError`.

**AC-5: `ISam2GraphHandleImpl.add_factor` real body** — calls `self._estimator._graph.add(factor)`; success logged; failure logged + raises `EstimatorDegradedError`.

**AC-6: `ISam2GraphHandleImpl.update` real body** — calls `self._estimator._isam2.update(graph, values)` AND `self._estimator._smoother.update(...)`; success logged; failure logged + raises `EstimatorFatalError`.

**AC-7: `ISam2GraphHandleImpl.compute_marginals` real body** — returns `gtsam.Marginals(self._estimator._isam2.getFactorsUnsafe(), self._estimator._isam2.getCurrentEstimate())`.

**AC-8: `ISam2GraphHandleImpl.last_anchor_age_ms` real body** — returns `(monotonic_ns() - self._estimator._last_anchor_ns) // 1e6`. Initialised to 0 (no anchor yet).

**AC-9: Defensive logging on every mutation** — `add_factor`/`update` log SUCCESS or FAILURE with structured fields (R05 mitigation).

**AC-10: `add_*` and `current_estimate` raise `NotImplementedError`** — with messages `"Factor adds owned by AZ-383"` / `"Marginals + outputs owned by AZ-384"`.

## Non-Functional Requirements

- Construction p99 ≤ 100 ms.
- `add_factor` p99 ≤ 1 ms (just appends to local graph).
- `update` p99 ≤ 30 ms steady state.
- `compute_marginals` p99 ≤ 60 ms (the dominant per-frame cost).

## Constraints

- Single-writer thread.
- iSAM2 + smoother are GTSAM-pinned (Plan-phase); both live in the same `gtsam` namespace.
- Defensive logging is mandatory (R05 — silent factor-add failure mitigation).
- `keyframe_window_size` MUST be in [10, 20] per D-C5-3.

## Risks & Mitigation

- **Risk: GTSAM `IncrementalFixedLagSmoother` API differs across pin versions** — Plan-phase pins lock the version.
- **Risk: Missing-key silent failure (R05)** — defensive logging on every mutation; AC-9 enforces.
- **Risk: `_isam2_handle.py` body replacement creates a merge-conflict with AZ-381** — AZ-381 skeleton ships first; this task strictly replaces method bodies; no Protocol changes.

## Runtime Completeness

- **Named capability**: `GtsamIsam2StateEstimator` skeleton + concrete `ISam2GraphHandleImpl` bodies.
- **Production code**: real iSAM2 + smoother construction; real handle method bodies that mutate the graph.
- **Unacceptable substitutes**: a `MockGraph` in production code; suppressing the defensive logs (R05).
