[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/03_implementation/batch_13_cycle1_report.md

@@ -0,0 +1,84 @@
+# Batch 13 — Cycle 1 Implementation Report
+
+**Batch**: 13 of N
+**Tasks landed**: AZ-382 (`GtsamIsam2StateEstimator` skeleton — iSAM2 + `IncrementalFixedLagSmoother` wiring + `ISam2GraphHandleImpl` real bodies)
+**Cycle**: 1
+**Date**: 2026-05-11
+
+## Scope
+
+| Task | Component | Purpose |
+|------|-----------|---------|
+| AZ-382 | C5 state estimator | Replaces the AZ-381 NotImplementedError skeleton for the iSAM2 graph handle with real GTSAM calls, and adds the production-default `GtsamIsam2StateEstimator` class that owns the `gtsam.ISAM2` + `gtsam_unstable.IncrementalFixedLagSmoother(K * frame_period_s)` substrate. The estimator's `StateEstimator` Protocol methods (`add_vio`, `add_pose_anchor`, `add_fc_imu`, `current_estimate`, `smoothed_history`, `health_snapshot`) intentionally still raise `NotImplementedError` — their bodies are owned by AZ-383 (factor adds) and AZ-384 (marginals + outputs). The `_isam2_handle.py` four-method surface (`add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms`), however, is fully implemented here, including defensive success/failure logging on every mutation (R05 mitigation). |
+
+## Files added / modified
+
+### Added (prod)
+
+- `src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py` — `GtsamIsam2StateEstimator` (`StateEstimator`) class + module-level `create(...)` factory + `register()` convenience. Constructor builds `_isam2 = gtsam.ISAM2(gtsam.ISAM2Params())`, `_smoother = gtsam_unstable.IncrementalFixedLagSmoother(K * _FRAME_PERIOD_S)`, `_graph = gtsam.NonlinearFactorGraph()`, `_values = gtsam.Values()`, `_key_for_frame: dict[UUID, int] = {}`, `_next_key_counter = 0`, `_last_anchor_ns = 0`. Emits one DEBUG log `kind="c5.state.isam2_initialised"`. Exposes `key_for_frame(frame_id) -> int` for AZ-383 key-management. The 6 Protocol methods raise `NotImplementedError` naming AZ-383 / AZ-384.
+
+### Modified (prod)
+
+- `src/gps_denied_onboard/components/c5_state/_isam2_handle.py` — replaces all four `NotImplementedError` bodies with real GTSAM calls:
+  - `add_factor(factor)` → `self._estimator._graph.add(factor)`; success → DEBUG log `c5.state.add_factor_ok` with `{factor_type, graph_size}`; failure → ERROR log `c5.state.add_factor_failed` + raise `EstimatorDegradedError`.
+  - `update(graph, values, timestamps=None)` → `self._estimator._isam2.update(graph, values)` then `self._estimator._smoother.update(graph, values, timestamps_map)`; `timestamps=None` substitutes an empty `gtsam_unstable.FixedLagSmootherKeyTimestampMap()`. Either failure → ERROR log + raise `EstimatorFatalError`.
+  - `compute_marginals()` → `gtsam.Marginals(self._estimator._isam2.getFactorsUnsafe(), self._estimator._isam2.calculateEstimate())`.
+  - `last_anchor_age_ms()` → `(time.monotonic_ns() - self._estimator._last_anchor_ns) // 1_000_000`.
+
+### Added (tests)
+
+- `tests/unit/c5_state/test_az382_isam2_smoother_wiring.py` — 27 tests covering all 10 ACs: substrate construction + DEBUG log; unique key assignment + idempotent re-lookup + `'x'` namespace; smoother window equals `K * frame_period_s`; window-bounds validation [10, 20] at config-load (`5` and `21` both rejected); `add_factor` success path + failure path with `EstimatorDegradedError` + structured failure log; `update` success path with iSAM2 estimate growing + smoother accepting empty timestamps map; `update` iSAM2 failure → `EstimatorFatalError`; `update` smoother failure → `EstimatorFatalError`; `compute_marginals` returns a `gtsam.Marginals`; `last_anchor_age_ms` returns very large initially and ~0 after anchor set; defensive logging on every mutation (success + failure paths); all six Protocol methods on the estimator raising `NotImplementedError` with `AZ-383` / `AZ-384` in the message; `register()` adds strategy to the factory registry; `create()` returns a handle bound to the returned estimator.
+
+### Modified (tests)
+
+- `tests/unit/c5_state/test_az381_state_protocol.py` — removed the now-obsolete `test_ac8_handle_methods_raise_named_task` (the bodies no longer raise `NotImplementedError`; they're real). Replaced with a comment pointing to the new AZ-382 AC test file. The Protocol-conformance assertion `test_ac8_handle_is_isam2_graph_handle` was retained.
+
+## Architectural notes
+
+- **`_FRAME_PERIOD_S = 1.0 / 3.0`** — D-C5-3 fixes the keyframe processing rate at 3 Hz (so a K=15 window equals 5 s of wall time). This is a module-level constant, not a config knob — only `K` (the keyframe count) is operator-tunable per the contract.
+- **Empty `FixedLagSmootherKeyTimestampMap` shim** — GTSAM's `IncrementalFixedLagSmoother.update(...)` does not have a no-timestamps overload (unlike `ISAM2.update`). The handle's `update(...)` substitutes an empty map when the caller passes `timestamps=None` so the C4 → C5 seam can stay `timestamps: Any | None = None` per the Protocol; AZ-383 will populate the map with per-key arrival timestamps.
+- **Defensive logging (R05)** — every mutation logs structured success or failure with `kind` keys: `c5.state.add_factor_ok`, `c5.state.add_factor_failed`, `c5.state.update_ok`, `c5.state.isam2_update_failed`, `c5.state.smoother_update_failed`. The C5 contract makes this mandatory because silent factor-add failures bit the prototype.
+- **AC-4 enforcement** — `keyframe_window_size ∈ [10, 20]` is enforced at `C5StateConfig.__post_init__` (AZ-381 ground), so AZ-382 inherits the gate "for free"; the AZ-382 AC tests assert via `C5StateConfig(keyframe_window_size=5)` raising `ConfigError`.
+- **`register()` is opt-in, not auto-import** — matches the C8 fc_factory pattern. Per-binary bootstrap modules and test fixtures call it explicitly; ADR-002 keeps the build-flag gate the single source of strategy availability.
+
+## Test counts
+
+| Suite | Before (B12) | After (B13) | Delta |
+|-------|--------------|-------------|-------|
+| Total passing | 521 | 547 | +26 |
+| Skipped | 2 | 2 | 0 |
+| AZ-382 (new) | 0 | 27 | +27 |
+| AZ-381 (preserved) | 20 | 19 | −1 (obsolete handle-NIE test removed; behaviour now lives in AZ-382 tests) |
+
+Run command: `PYTHONPATH=src pytest tests/ -q` → `547 passed, 2 skipped in ~30s`.
+
+## Lint / type
+
+- `ruff check src/ tests/` — clean (1 fixable issue auto-corrected during the cycle).
+- `ruff format src/ tests/` — clean (1 file reformatted).
+- `mypy` on the new modules — 0 errors. Pre-existing errors in `logging/structured.py` + `c13_fdr/writer.py` left untouched per scope discipline.
+
+## Acceptance evidence
+
+| AC | Test(s) | Status |
+|----|--------|--------|
+| AC-1 Construction | `test_ac1_construction_initialises_substrate`, `test_ac1_construction_emits_debug_log` | PASS |
+| AC-2 Key-management | `test_ac2_key_for_frame_assigns_unique_keys`, `test_ac2_key_for_frame_is_idempotent`, `test_ac2_keys_use_x_namespace` | PASS |
+| AC-3 Window size | `test_ac3_smoother_window_is_k_times_frame_period` | PASS |
+| AC-4 Window validation | `test_ac4_window_below_min_rejected_by_config`, `test_ac4_window_above_max_rejected_by_config` | PASS |
+| AC-5 `add_factor` real body | `test_ac5_add_factor_appends_to_graph`, `test_ac5_add_factor_failure_raises_degraded_and_logs` | PASS |
+| AC-6 `update` real body | `test_ac6_update_advances_isam2_and_smoother`, `test_ac6_update_with_none_timestamps_substitutes_empty_map`, `test_ac6_isam2_failure_raises_fatal`, `test_ac6_smoother_failure_raises_fatal` | PASS |
+| AC-7 `compute_marginals` real body | `test_ac7_compute_marginals_returns_real_instance` | PASS |
+| AC-8 `last_anchor_age_ms` | `test_ac8_last_anchor_age_ms_huge_when_no_anchor`, `test_ac8_last_anchor_age_ms_small_after_anchor_set` | PASS |
+| AC-9 Defensive logging | `test_ac9_add_factor_emits_success_log`, `test_ac9_update_emits_success_log`, plus failure-path log assertions in AC-5/AC-6 tests | PASS |
+| AC-10 `NotImplementedError` on estimator methods | `test_ac10_add_vio_raises_named_az383`, `test_ac10_add_pose_anchor_raises_named_az383`, `test_ac10_add_fc_imu_raises_named_az383`, `test_ac10_current_estimate_raises_named_az384`, `test_ac10_smoothed_history_raises_named_az384`, `test_ac10_health_snapshot_raises_named_az384` | PASS |
+
+## Caplog gotcha discovered
+
+`get_logger(name)` unconditionally sets `logger.setLevel(logging.NOTSET)`, which silently overrides any `caplog.at_level(logging.DEBUG, logger=<name>)` set by a test BEFORE the logger is fetched. Symptom: DEBUG records vanish from `caplog.records`. Workaround in this batch: tests that need to capture construction-time DEBUG logs use `caplog.at_level(logging.DEBUG)` at the root level. Worth a project-wide rule once we hit the same in another component — but not in this batch's scope.
+
+## Known forward actions (not in scope this batch)
+
+- AZ-383 will populate `add_vio` / `add_pose_anchor` / `add_fc_imu` and start filling the `FixedLagSmootherKeyTimestampMap` with real per-key timestamps + recording `_last_anchor_ns` on every satellite-anchored pose.
+- AZ-384 will populate `current_estimate` / `smoothed_history` / `health_snapshot` against the marginals computed by the handle.
+- A per-binary bootstrap module (one per `BUILD_STATE_*` flag combination) will call `gtsam_isam2_estimator.register()` instead of relying on test fixtures.