gps-denied-onboard/_docs/02_tasks/todo/AZ-384_c5_marginals_outputs.md

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.