[AZ-384] C5 marginals + current_estimate/smoothed_history/health_snapshot

Replaces the last three NotImplementedError placeholders on
GtsamIsam2StateEstimator with real Marginals + output methods:

- current_estimate(): recovers the 6x6 Marginals covariance for the
  most-recently committed pose key, enforces the SPD invariant via
  np.linalg.cholesky (Invariant 10), converts the local-ENU pose
  translation to WGS84 via the shared WgsConverter, derives a
  body->world quaternion, and emits a fresh EstimatorOutput
  (smoothed=False, Invariant 4). On SPD failure transitions
  isam2_state -> LOST and raises EstimatorFatalError (AC-5.2 path).
- smoothed_history(n): iterates the smoother's active POSE keys via
  _smoother.calculateEstimate().keys() (filtered by GTSAM symbol
  char) and the smoother timestamps via ts_map.at(key) - workaround
  for the pinned gtsam_unstable build's non-iterable
  FixedLagSmootherKeyTimestampMap. Bounded by K (Invariant 6); every
  entry has smoothed=True (Invariant 7).
- health_snapshot(): cheap O(1) accumulator read; reports
  IsamState lifecycle, pose-key count, AC-NEW-8
  cov_norm_growing_for_s rolling 60s deque-backed counter, and
  spoof_promotion_blocked via the AZ-385 state machine injection
  point.

Adds two public injection points for AZ-385/composition root:
set_enu_origin(LatLonAlt) and attach_source_label_state_machine(machine).
Defaults: (0, 0, 0) ENU origin, VISUAL_PROPAGATED source label,
spoof_promotion_blocked=False.

Wires _record_committed_pose_key into the three add_* success paths
so current_estimate only reads keys that have real values in iSAM2.
The JACOBIAN path in add_pose_anchor deliberately skips this call -
Invariant 3 keeps the JACOBIAN pose out of the iSAM2 graph.

Tests: +27 in tests/unit/c5_state/test_az384_marginals_outputs.py
covering all 10 ACs. Three obsolete AZ-382 tests
(test_ac10_*_raises_named_az384) removed. Full suite: 589 passed,
2 skipped.

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

_docs/02_tasks/todo/AZ-384_c5_marginals_outputs.md

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