# C5 GtsamIsam2StateEstimator — Marginals + current_estimate / smoothed_history / health_snapshot

**Task**: AZ-384_c5_marginals_outputs
**Name**: C5 `GtsamIsam2StateEstimator` — Marginals + output methods
**Description**: Implement `current_estimate()`, `smoothed_history(n)`, and `health_snapshot()` on `GtsamIsam2StateEstimator`. `current_estimate()`: get the current pose key from the most-recent frame; recover the 6×6 covariance via `_isam2_handle.compute_marginals().marginalCovariance(pose_key)`; convert local-tangent-plane pose to WGS84 via `WgsConverter`; assemble `EstimatorOutput(smoothed=False, source_label = <gate state>, last_satellite_anchor_age_ms = handle.last_anchor_age_ms())`. `smoothed_history(n)`: return up to `min(n, K)` smoothed past keyframes from `IncrementalFixedLagSmoother.calculateEstimate()` projection; each entry has `smoothed=True`. `health_snapshot()`: report `IsamState` (INIT/TRACKING/DEGRADED/LOST) based on convergence quality; `keyframe_count = len(_smoother.timestamps())`; `cov_norm_growing_for_s` (rolling counter incremented when frame-to-frame cov norm rises monotonically per AC-NEW-8); `spoof_promotion_blocked` (queries the source-label state machine — owned by AZ-385; this task introduces a stub that returns False until AZ-385 lands). SPD-invariant defensive check on every emitted covariance.
**Complexity**: 3 points
**Dependencies**: AZ-383 (graph populated with factors), AZ-382 / AZ-381 (handle + Protocol), AZ-279 (`WgsConverter`), AZ-277 (`SE3Utils`), AZ-263, AZ-269, AZ-266, AZ-272
**Component**: c5_state (epic AZ-260 / E-C5)
**Tracker**: AZ-384
**Epic**: AZ-260 (E-C5)

### Document Dependencies

- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — Invariants 4 (current_estimate fresh), 6/7 (smoothed history bounded + flagged), 10 (SPD).
- `_docs/02_document/components/07_c5_state/description.md` — § 2 outputs; § 7 Marginals dominant cost.

## Problem

Without this task, the system has no way to read the posterior pose; downstream C8 cannot emit FC corrections; FDR has no smoothed history.

## Outcome

- `current_estimate()` body: get current pose + Marginals 6×6; WGS84-convert via helper; assemble `EstimatorOutput`.
- `smoothed_history(n)` body: iterate smoother's active keyframes; build `EstimatorOutput(smoothed=True)` for each.
- `health_snapshot()` body: `IsamState` derivation + `keyframe_count` + `cov_norm_growing_for_s` rolling counter + `spoof_promotion_blocked` (from injected source-label state machine; default `False` until AZ-79).
- `_cov_norm_window` private rolling-window counter (60 s lazy-pruned) for AC-NEW-8 monotonicity check.
- SPD-invariant defensive check before every `EstimatorOutput` emission; on failure raise `EstimatorFatalError`.
- Constructor extension: optional `source_label_state_machine` arg (default `None`; AZ-79 wires it up).

## Scope

### Included
- All three method bodies.
- `_cov_norm_window` rolling-window counter.
- SPD defensive check.
- Source-label state machine injection point (Optional, default None).
- Unit tests: synthetic graph with known pose → `current_estimate()` returns expected pose+covariance; `smoothed_history(20)` bounded by K=15; SPD invariant; `IsamState` derivation; `cov_norm_growing_for_s` monotonicity counter accuracy.

### Excluded
- Source-label state machine impl — owned by AZ-79.
- Spoof gate logic body — owned by AZ-79.
- AC-5.2 fallback — owned by AZ-81.
- ESKF baseline — owned by AZ-80.

## Acceptance Criteria

**AC-1: `current_estimate` returns fresh `EstimatorOutput`** — every call returns a new instance with `smoothed=False`.

**AC-2: SPD covariance** — `np.linalg.cholesky(out.covariance_6x6)` succeeds for every emitted output; non-SPD raises `EstimatorFatalError`.

**AC-3: WGS84 conversion** — uses shared `WgsConverter`; output matches helper test vectors.

**AC-4: `smoothed_history(n)` bounded by K** — `len(smoothed_history(100)) <= K=15`; each has `smoothed=True`.

**AC-5: `current_estimate` has `smoothed=False`** — distinguishes from history.

**AC-6: `health_snapshot.isam2_state` matches convergence quality** — INIT before first factor; TRACKING after; DEGRADED on inflated cov; LOST on `EstimatorFatalError`.

**AC-7: `keyframe_count` accuracy** — matches `IncrementalFixedLagSmoother.timestamps().size()`.

**AC-8: `cov_norm_growing_for_s`** — increments while consecutive frames show monotone-rising cov norm; resets to 0 on a non-rising frame.

**AC-9: `spoof_promotion_blocked` via injected state machine** — queries `source_label_state_machine.is_spoof_promotion_blocked()`; default `False` if no state machine wired.

**AC-10: `last_satellite_anchor_age_ms` pass-through** — every `EstimatorOutput.last_satellite_anchor_age_ms == handle.last_anchor_age_ms()`.

## Non-Functional Requirements

- `current_estimate` p95 ≤ 60 ms (Marginals dominant).
- `smoothed_history(K)` p99 ≤ 20 ms.
- `health_snapshot` p99 ≤ 5 µs (O(1) accumulator reads).

## Constraints

- Single-writer thread.
- SPD defensive check is mandatory.
- `WgsConverter` use is mandatory (no inline math).

## Risks & Mitigation

- **Risk: `IncrementalFixedLagSmoother.calculateEstimate()` returns full Values not just keyframe poses** — filter by key; verify against pinned GTSAM API.
- **Risk: SPD invariant fails under iSAM2 numerical instability** — defensive raise (AC-2) maps to `EstimatorFatalError`; AC-5.2 fallback then triggers.

## Runtime Completeness

- **Named capability**: posterior pose + covariance recovery + smoothed history.
- **Production code**: real Marginals, real WGS84 conversion, real rolling-window counter, real SPD defensive check.
- **Unacceptable substitutes**: synthetic Marginals; inline WGS84 math.