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