[AZ-382] C5 GtsamIsam2StateEstimator skeleton + real iSAM2 handle bodies

- Add GtsamIsam2StateEstimator owning the GTSAM substrate:
  gtsam.ISAM2(ISAM2Params()) + gtsam_unstable.IncrementalFixedLagSmoother
  (K * 1/3 s window per D-C5-3) + NonlinearFactorGraph + Values.
- Module-level create(...) factory + register() helper for
  register_state_estimator("gtsam_isam2", create). Opt-in registration
  per ADR-002 — no auto-import.
- Key-management policy: key_for_frame(UUID) -> int via
  gtsam.symbol('x', counter); idempotent re-lookup.
- Replace all four NotImplementedError bodies in _isam2_handle.py with
  real GTSAM calls:
  * add_factor → estimator._graph.add(factor); R05 defensive logging
    on success/failure; EstimatorDegradedError on failure.
  * update → _isam2.update + _smoother.update; empty
    FixedLagSmootherKeyTimestampMap substituted for timestamps=None;
    EstimatorFatalError on either failure.
  * compute_marginals → gtsam.Marginals(getFactorsUnsafe(),
    calculateEstimate()).
  * last_anchor_age_ms → (monotonic_ns - _last_anchor_ns) // 1e6.
- StateEstimator Protocol methods on the estimator still raise
  NotImplementedError naming AZ-383 (factor adds) / AZ-384
  (marginals + outputs).
- AZ-382 AC tests: 27 cases covering 10/10 ACs + factory integration.
- AZ-381 test_ac8_handle_methods_raise_named_task removed (obsolete:
  bodies are real now); test_ac8_handle_is_isam2_graph_handle retained.
- Full suite: 547 passed (+26 vs B12), 2 skipped.
- Impl report: _docs/03_implementation/batch_13_cycle1_report.md.

Co-authored-by: Cursor <cursoragent@cursor.com>

_docs/02_tasks/done/AZ-382_c5_isam2_smoother_wiring.md

@@ -0,0 +1,101 @@
+# 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).