# Batch 15 — Cycle 1 Implementation Report

**Batch**: 15 of N
**Tasks landed**: AZ-384 (`GtsamIsam2StateEstimator` — Marginals + output methods)
**Cycle**: 1
**Date**: 2026-05-11

## Scope

| Task | Component | Purpose |
|------|-----------|---------|
| AZ-384 | C5 state estimator | Replaces the three remaining `NotImplementedError` placeholders on `GtsamIsam2StateEstimator` (`current_estimate`, `smoothed_history`, `health_snapshot`) with real implementations. `current_estimate` recovers the 6x6 Marginals covariance for the most-recently committed pose key, enforces the SPD invariant via Cholesky decomposition (Invariant 10), converts the local-ENU pose translation to WGS84 via the shared `WgsConverter`, derives a body→world quaternion from the iSAM2 pose, and emits a fresh `EstimatorOutput(smoothed=False)` (Invariant 4). `smoothed_history(n)` iterates the smoother's active POSE keys (filtered by GTSAM symbol char), sorts by per-key timestamp from `IncrementalFixedLagSmoother.timestamps()`, takes the most recent `min(n, K)` entries, and emits `EstimatorOutput(smoothed=True)` for each (Invariant 6 + 7). `health_snapshot` returns an O(1) accumulator read — `IsamState` lifecycle, pose-key count from the smoother estimate, AC-NEW-8 `cov_norm_growing_for_s` rolling-window counter, and the spoof-promotion gate query through the (still-pending AZ-385) state machine injection point. |

## Files added / modified

### Modified (prod)

- `src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py` — replaced the three Protocol `NotImplementedError` bodies with real Marginals + output method bodies; introduced six new state fields on `GtsamIsam2StateEstimator` (`_last_committed_pose_key`, `_enu_origin`, `_source_label_machine`, `_cov_norm_window`, `_isam2_state`); added two public injection methods (`set_enu_origin`, `attach_source_label_state_machine`); added seven internal helpers (`_record_committed_pose_key`, `_pose_at_key`, `_enu_pose_to_wgs84`, `_latest_velocity_or_zero`, `_derive_source_label`, `_spoof_promotion_blocked`, `_smoother_keyframe_count`, `_record_cov_norm_sample`, `_prune_cov_norm_window`, `_cov_norm_growing_for_s`); wired `_record_committed_pose_key` into the three `add_*` success paths (sets the key the next `current_estimate` will read); added two module-level pure helpers (`_quat_from_pose3`, `_enforce_spd`); added module-level constants `_COV_NORM_WINDOW_NS = 60 * 1e9` ns and `_DEFAULT_ENU_ORIGIN = LatLonAlt(0, 0, 0)`.

### Added (tests)

- `tests/unit/c5_state/test_az384_marginals_outputs.py` — 27 tests covering all 10 ACs. The shared `_seed_prior` helper plants a real prior factor + initial value + timestamp map onto the iSAM2 + smoother — the minimal scaffolding `current_estimate` needs (AZ-388 will own this seeding at startup once the AC-5.2 fallback lands).

### Modified (tests)

- `tests/unit/c5_state/test_az382_isam2_smoother_wiring.py` — removed the three now-obsolete `test_ac10_*_raises_named_az384` tests (`current_estimate` / `smoothed_history` / `health_snapshot` no longer raise `NotImplementedError`; they have real bodies). Replaced with a comment block pointing readers to both `test_az383_factor_adds.py` and `test_az384_marginals_outputs.py`.

## Architectural notes

- **`_last_committed_pose_key` tracking** — set ONLY after a successful `handle.update` that committed a value for the key. The JACOBIAN path in `add_pose_anchor` deliberately does NOT call `_record_committed_pose_key` because Invariant 3 forbids adding the JACOBIAN pose to the iSAM2 graph (the running estimate consumes it downstream, but the graph stops growing under throttle). This means `current_estimate` reads only keys that have ACTUAL values in iSAM2 — no missing-key surprises in the steady state.
- **ENU origin as injection point** — `set_enu_origin(LatLonAlt)` is the wiring seam for AZ-385 (which will derive the origin from the first satellite-anchored pose via the spoof-promotion gate). Before the wire-up, the estimator falls back to `_DEFAULT_ENU_ORIGIN = (0, 0, 0)` — correct for tests, obviously not for flight; the composition root SHOULD call `set_enu_origin` before steady-state operation.
- **GTSAM iteration via `calculateEstimate().keys()` + `timestamps().at(key)`** — `FixedLagSmootherKeyTimestampMap` is not iterable in the pinned `gtsam_unstable` build (no `items()`, no `keys()`, no `__iter__`). The workaround: iterate `_smoother.calculateEstimate().keys()` (which returns the smoother's active values, bounded by the K-keyframe window), filter to `'x'`-namespace pose keys via `gtsam.symbolChr(key) == ord("x")`, then probe per-key timestamps with `ts_map.at(int(key))`. Documented inline as a forward-compat note since a future GTSAM version may expose `items()` directly.
- **Smoother estimate vs. iSAM2 estimate** — `current_estimate` reads from `_isam2.calculateEstimate()` (the filter posterior — the most up-to-date best estimate). `smoothed_history` reads from `_smoother.calculateEstimate()` (the smoother posterior — past keyframes re-optimised given all later evidence). This split matches the C5 contract semantics: `smoothed=True` entries are genuinely smoothed; `smoothed=False` entries are filter-only.
- **SPD invariant enforcement (Invariant 10)** — `_enforce_spd` uses `np.linalg.cholesky` (same primitive iSAM2 uses internally). On `LinAlgError` we transition `_isam2_state = LOST`, log a structured `c5.state.current_estimate_spd_failed` record with the covariance Frobenius norm for forensics, and raise `EstimatorFatalError` — which triggers the AC-5.2 IMU-only fallback path in C8 once AZ-388 lands.
- **AC-NEW-8 `cov_norm_growing_for_s` rolling window** — implemented as a `deque[(monotonic_ns, fro_norm)]` lazy-pruned to a 60 s window on every `_record_cov_norm_sample` call. The "growing for" length is computed by walking the deque newest → oldest until the chain breaks (a sample whose norm is ≤ its successor). Cheap O(n) per snapshot, n bounded by the keyframe rate × 60 s.
- **Source-label state machine — injection point only** — `attach_source_label_state_machine` accepts any object exposing `current_label() -> PoseSourceLabel` and `is_spoof_promotion_blocked() -> bool`. AZ-385 owns the actual state-machine impl + transition logic. Defaults: `VISUAL_PROPAGATED` label and `spoof_promotion_blocked = False`. Exceptions raised by the injected machine are logged but downgraded to the defaults so a flaky state machine does NOT take down the estimator.
- **No marginals call in `health_snapshot`** — the contract NFR is `health_snapshot` p99 ≤ 5 µs. Strictly accumulator reads + a single `calculateEstimate().keys()` walk for the pose-key count. Verified by an explicit "no compute_marginals call" test.

## Test counts

| Suite | Before (B14) | After (B15) | Delta |
|-------|--------------|-------------|-------|
| Total passing | 565 | 589 | +24 |
| Skipped | 2 | 2 | 0 |
| AZ-384 (new) | 0 | 27 | +27 |
| AZ-382 (preserved) | 24 | 21 | −3 (obsolete `test_ac10_*_raises_named_az384` tests removed; behaviour now lives in AZ-384 tests) |

Run command: `PYTHONPATH=src pytest tests/ -q` → `589 passed, 2 skipped in ~19s`.

## Lint / type

- `ruff check src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/` — clean.
- `ruff format` — one file reformatted, 12 already formatted.
- `ReadLints` on touched files — 0 errors.

## Acceptance evidence

| AC | Test(s) | Status |
|----|---------|--------|
| AC-1 Fresh `EstimatorOutput` | `test_ac1_current_estimate_returns_fresh_estimator_output`, `test_ac1_no_committed_pose_key_raises_fatal` | PASS |
| AC-2 SPD covariance | `test_ac2_spd_invariant_holds_for_real_marginals`, `test_ac2_non_spd_marginals_raises_fatal` | PASS |
| AC-3 WGS84 conversion | `test_ac3_default_origin_is_equator`, `test_ac3_explicit_origin_round_trips`, `test_ac3_translated_pose_offsets_from_origin` | PASS |
| AC-4 `smoothed_history` bounded by K | `test_ac4_smoothed_history_bounded_by_k`, `test_ac4_smoothed_history_entries_have_smoothed_true`, `test_ac4_smoothed_history_empty_when_n_zero`, `test_ac4_smoothed_history_empty_before_seed` | PASS |
| AC-5 `current_estimate` has `smoothed=False` | `test_ac5_current_estimate_smoothed_false` | PASS |
| AC-6 `isam2_state` lifecycle | `test_ac6_isam2_state_init_before_first_estimate`, `test_ac6_isam2_state_tracking_after_estimate`, `test_ac6_isam2_state_lost_after_fatal` | PASS |
| AC-7 `keyframe_count` accuracy | `test_ac7_keyframe_count_initially_zero`, `test_ac7_keyframe_count_grows_with_seeded_keys` | PASS |
| AC-8 `cov_norm_growing_for_s` | `test_ac8_cov_norm_growing_zero_with_constant_norm`, `test_ac8_cov_norm_growing_increments_under_rising_norm`, `test_ac8_cov_norm_growing_resets_on_drop` | PASS |
| AC-9 `spoof_promotion_blocked` via state machine | `test_ac9_default_spoof_promotion_blocked_false`, `test_ac9_spoof_promotion_blocked_from_state_machine`, `test_ac9_state_machine_drives_source_label`, `test_ac9_default_source_label_is_visual_propagated` | PASS |
| AC-10 `last_satellite_anchor_age_ms` pass-through | `test_ac10_last_satellite_anchor_age_ms_passthrough`, `test_ac10_emitted_at_is_monotonic_ns` | PASS |
| Defensive — cheap `health_snapshot` | `test_health_snapshot_does_not_call_marginals` | PASS |

## Known forward actions (not in scope this batch)

- **AZ-385 (source-label + spoof-promotion gate)** — owns the `SourceLabelStateMachine` implementation that AZ-384 only holds a reference to. Will also wire `set_enu_origin` to the first satellite-anchored pose.
- **AZ-386 (ESKF baseline)** — the mandatory simple-baseline `EskfStateEstimator` (IT-12 engine rule). Independent of AZ-384; shares the same `EstimatorOutput` / `EstimatorHealth` DTOs.
- **AZ-387 (smoothed history → FDR)** — the C13 FDR writer path. AZ-384's `smoothed_history(n)` is the input.
- **AZ-388 (AC-5.2 fallback)** — owns the startup `add_vio`-first-frame seeding so the full real-iSAM2 ↔ AZ-383 ↔ AZ-384 chain runs end-to-end. The AZ-384 test fixture's `_seed_prior` helper mirrors what AZ-388 will do at composition-root startup.
- **`_latest_velocity_or_zero`** — currently returns zeros until an IMU keyframe has committed a velocity value via `add_fc_imu`. The actual nonzero-velocity case will exercise once AZ-388 lands first-frame seeding for the IMU chain.