Decompose Step 6 snapshot: 140 task specs + contract docs

Closes out greenfield Step 6 (Decompose) for all 14 components
(C1-C13 + cross-cutting helpers/replay). Covers tasks AZ-266..AZ-446
plus the _dependencies_table.md and component contract documents.

State file updated to greenfield Step 7 (Implement), not_started.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 00:39:48 +03:00
parent 8171fcb29e
commit 880eabcb3f
172 changed files with 22897 additions and 35 deletions
@@ -0,0 +1,88 @@
# 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.