[AZ-381] C5 StateEstimator protocol + factory + C8 DTO reshape

- Add StateEstimator Protocol (6 methods, @runtime_checkable) + DTOs
  (EstimatorOutput, EstimatorHealth, IsamState, PoseSourceLabel, Quat)
  in _types/state.py per state_estimator_protocol.md v1.0.0.
- Add C5 error hierarchy (StateEstimatorError + 3 subclasses) and
  C5StateConfig (strategy, keyframe_window, spoof gates,
  no_estimate_fallback_s) with __post_init__ validation.
- Add ISam2GraphHandle Protocol + ISam2GraphHandleImpl skeleton (all
  4 methods raise NotImplementedError naming AZ-382 as owner).
- Add build_state_estimator factory + bind_state_ingest_thread for
  single-writer enforcement; ADR-002 build-flag gating
  (BUILD_STATE_<variant>); INFO log on success.
- Strict reshape of legacy EstimatorOutput / EstimatorHealth across
  all 6 C8 production files (_outbound_provenance,
  _covariance_projector, pymavlink_ardupilot_adapter,
  msp2_inav_adapter, mavlink_gcs_adapter, interface) + 6 C8 test
  files (UUID frame_id, LatLonAlt position_wgs84, Quat orientation,
  PoseSourceLabel enum source_label). Remove ad-hoc DTOs from
  _types/pose.py and from C4's public __init__ (EstimatorOutput is a
  C5 concept, not a C4 one).
- 20 AZ-381 AC tests (10 ACs + 4 config range + NFR + conformance).
- Full suite: 521 passed, 2 skipped (+20 vs Batch 11).
- Contracts: state_estimator_protocol.md v1.0.0 -> active;
  composition_root_protocol.md v1.2.0 -> v1.3.0 (additive state
  block + factory + ingest-thread binding).
- Impl report: _docs/03_implementation/batch_12_cycle1_report.md.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 05:35:20 +03:00
parent 8a9cf88a46
commit beed43724f
32 changed files with 1394 additions and 157 deletions
@@ -4,8 +4,8 @@
**Producer task**: AZ-381 (Protocol + DTOs + factory + composition + concrete `ISam2GraphHandle`)
**Consumer tasks**: AZ-382 (iSAM2 + IncrementalFixedLagSmoother wiring), AZ-383 (Factor adds), AZ-384 (Marginals + outputs), AZ-385 (Source-label + spoof gate), AZ-386 (ESKF baseline), AZ-387 (Smoothed history → FDR), AZ-388 (AC-5.2 fallback), AZ-389 (Orthorectifier → C6).
**Version**: 1.0.0
**Status**: draft
**Last Updated**: 2026-05-10
**Status**: active
**Last Updated**: 2026-05-11
**Module-layout home**: `src/gps_denied_onboard/components/c5_state/interface.py`, `src/gps_denied_onboard/components/c5_state/__init__.py`, `src/gps_denied_onboard/runtime_root/state_factory.py`
## Purpose
@@ -3,7 +3,7 @@
**Component**: shared_config (cross-cutting concern owned by E-CC-CONF / AZ-246)
**Producer tasks**: AZ-269 (config loader + outer Config) and AZ-270 (compose_root + compose_operator + StrategyNotLinkedError)
**Consumer tasks**: every component task that takes a config block; `runtime_root.py` and `operator_tool/__main__.py` (the two composition-root entrypoints)
**Version**: 1.2.0
**Version**: 1.3.0
**Status**: draft
**Last Updated**: 2026-05-11
@@ -120,3 +120,4 @@ change (AC-NEW-3 / RESTRICT-UAV-4).
| 1.0.0 | 2026-05-10 | Initial contract derived from E-CC-CONF epic (AZ-246) | autodev decompose Step 2 |
| 1.1.0 | 2026-05-11 | Add takeoff sequence section + `EXIT_FDR_OPEN_FAILURE` (AZ-296) | autodev batch 7 |
| 1.2.0 | 2026-05-11 | Add cross-cutting `fc` (`FcConfig`) and `gcs` (`GcsConfig`) blocks + `build_fc_adapter` / `build_gcs_adapter` factories + outbound-thread single-writer binding (AZ-390) | autodev batch 8 |
| 1.3.0 | 2026-05-11 | Add `state` (`C5StateConfig`) block + `build_state_estimator(config, *, imu_preintegrator, se3_utils, wgs_converter, fdr_client) -> (StateEstimator, ISam2GraphHandle)` factory + state-ingest-thread single-writer binding (`bind_state_ingest_thread`); state factory MUST be invoked BEFORE C4 `build_pose_estimator` so the returned `ISam2GraphHandle` can be injected into C4 (AZ-381) | autodev batch 12 |
@@ -0,0 +1,126 @@
# Batch 12 — Cycle 1 Implementation Report
**Batch**: 12 of N
**Tasks landed**: AZ-381 (C5 `StateEstimator` Protocol + DTOs + factory + concrete `ISam2GraphHandle` skeleton + strict EstimatorOutput/EstimatorHealth reshape across C8)
**Cycle**: 1
**Date**: 2026-05-11
## Scope
| Task | Component | Purpose |
|------|-----------|---------|
| AZ-381 | C5 state estimator + composition root + C8 fc_adapter migration | Defines the public C5 surface — `StateEstimator` Protocol (6 methods, `@runtime_checkable`), DTOs (`EstimatorOutput`, `EstimatorHealth`, `IsamState`, `PoseSourceLabel`, `Quat`), error hierarchy (`StateEstimatorError` + 3 subclasses), `C5StateConfig`, `ISam2GraphHandleImpl` skeleton (body owned by AZ-382), and `build_state_estimator(...) -> (StateEstimator, ISam2GraphHandle)` factory with single-ingest-thread binding. Per ADR-002, strategy resolution is build-flag-gated (`BUILD_STATE_<variant>`). Reshapes `EstimatorOutput` to the v1.0.0 contract (UUID frame_id, `LatLonAlt` position, `Quat` orientation, `PoseSourceLabel` enum, `npt.NDArray` covariance, `smoothed: bool`); reshapes `EstimatorHealth` to `IsamState` + spoof-gate fields. Migrates all six C8 production files (`_outbound_provenance`, `_covariance_projector`, `pymavlink_ardupilot_adapter`, `msp2_inav_adapter`, `mavlink_gcs_adapter`, `interface`) and 50+ unit tests to the new DTO shape. |
## Strategic decision: strict reshape (option A)
Before any code was written, a contract-vs-reality conflict was surfaced: the v1.0.0 C5 contract specified a fundamentally different `EstimatorOutput` shape than the ad-hoc one C8 had been carrying (legacy `int` frame_id, `pose_se3`, `extras["wgs84"]`). Three options were presented to the user (strict reshape — match contract; split reshape — partial; revise contract). The user chose **strict reshape**: migrate all six C8 prod files + every C8 test to match the v1.0.0 contract in one batch. This trades batch size (≈10pt) for zero contract debt going into AZ-382.
## Files added / modified
### Added (prod)
- `src/gps_denied_onboard/_types/state.py``EstimatorOutput`, `EstimatorHealth`, `IsamState`, `PoseSourceLabel`, `Quat`. `frozen=True, slots=True` per AC-2. Numpy typing via `npt.NDArray[Any]` to keep numpy a `TYPE_CHECKING`-only import (DTO doesn't actually need numpy at runtime — covariance values flow through).
- `src/gps_denied_onboard/components/c5_state/errors.py``StateEstimatorError` (base) + `EstimatorDegradedError`, `EstimatorFatalError`, `StateEstimatorConfigError`. AC-10: every subclass is `except StateEstimatorError`-catchable.
- `src/gps_denied_onboard/components/c5_state/config.py``C5StateConfig` (frozen) with five fields per the contract: `strategy`, `keyframe_window_size` (clamped 160), `spoof_promotion_min_stable_s` (>0), `spoof_promotion_visual_consistency_tol_m` (>0), `no_estimate_fallback_s` (>0). `__post_init__` validates and raises `StateEstimatorConfigError` on out-of-range or unknown strategy (AC-5).
- `src/gps_denied_onboard/components/c5_state/_isam2_handle.py``ISam2GraphHandle` Protocol (`@runtime_checkable`) + `ISam2GraphHandleImpl` skeleton; all four methods raise `NotImplementedError("Body owned by AZ-382 iSAM2 wiring task")` (AC-8). The class is intentionally underscore-prefixed and not re-exported.
- `src/gps_denied_onboard/runtime_root/state_factory.py``build_state_estimator(config, *, imu_preintegrator, se3_utils, wgs_converter, fdr_client) -> (StateEstimator, ISam2GraphHandle)` + `bind_state_ingest_thread(thread_ident)` (single-writer enforcement). Build-flag check via env (`BUILD_STATE_<variant>`); lazy-import per ADR-002. Emits one INFO log `kind="c5.state.strategy_loaded"` with `{strategy, keyframe_window_size}` (AC-6).
### Added (tests)
- `tests/unit/c5_state/test_az381_state_protocol.py` — 20 tests covering all 10 ACs (Protocol runtime-checkable, missing-method failure path, frozen+slots, IsamState 4 values, build-flag rejection, unknown-strategy rejection at config + at factory, factory tuple + INFO log, second-thread-binding rejection, same-thread idempotent rebind, handle is `ISam2GraphHandle`, handle methods name AZ-382, public API re-exports, internals not in `__all__`, every error catchable by `StateEstimatorError`) + 4 config range tests (keyframe_window, spoof_min_stable, no_estimate_fallback) + 1 NFR `build_state_estimator` p99 ≤ 50 ms.
### Modified (prod — DTO consolidation)
- `src/gps_denied_onboard/_types/pose.py` — removed legacy `EstimatorOutput` + `EstimatorHealth`; only `PoseEstimate` remains. C5 owns the new shape in `_types/state.py`.
- `src/gps_denied_onboard/_types/emitted.py``EmittedExternalPosition.source_label` changed from `str` to `PoseSourceLabel`.
- `src/gps_denied_onboard/components/c4_pose/__init__.py` + `interface.py` — removed the re-export of `EstimatorOutput` from C4 (was an architectural smell — `EstimatorOutput` is a C5 concept). C4 now exposes only `PoseEstimate` + `PoseEstimator`.
- `src/gps_denied_onboard/components/c5_state/__init__.py` — registers `C5StateConfig` and re-exports `StateEstimator`, `EstimatorOutput`, `EstimatorHealth`, `IsamState`, `PoseSourceLabel`, plus the error hierarchy.
- `src/gps_denied_onboard/components/c5_state/interface.py` — full Protocol body with all 6 methods (`add_vio`, `add_pose_anchor`, `add_fc_imu`, `current_estimate`, `smoothed_history`, `health_snapshot`); typed against new DTOs.
### Modified (prod — C8 migration to new DTOs)
- `src/gps_denied_onboard/components/c8_fc_adapter/_outbound_provenance.py``SOURCE_LABEL_TO_FLOAT` keys now use `PoseSourceLabel.*.value`. `source_label_to_float` + `StatusTextTransitionRateLimiter` accept either `PoseSourceLabel` or string (back-compat shim for tests that pass enum members directly).
- `src/gps_denied_onboard/components/c8_fc_adapter/_covariance_projector.py` — imports `EstimatorOutput` from `_types/state`; FDR violation records cast UUID → str.
- `src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py``_extract_wgs84` now reads `output.position_wgs84` directly (no more `extras["wgs84"]`); UUID stringified in log records.
- `src/gps_denied_onboard/components/c8_fc_adapter/msp2_inav_adapter.py` — same migration pattern.
- `src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py` — same migration pattern.
- `src/gps_denied_onboard/components/c8_fc_adapter/interface.py` — imports `EstimatorOutput` from `_types/state`.
- `src/gps_denied_onboard/runtime_root/__init__.py` — exports `build_state_estimator`, `bind_state_ingest_thread`, `StateIngestThreadAlreadyBoundError` alongside the existing FC/GCS factory surface.
### Modified (tests — C8 + C5 + C4)
- `tests/unit/c8_fc_adapter/test_az390..test_az397_*.py` (6 files) — every `_make_output` / `_output` helper rebuilt to construct the new `EstimatorOutput` (UUID frame_id, `LatLonAlt(...)` position_wgs84, `Quat(...)` orientation, `velocity_world_mps` tuple, `PoseSourceLabel.*` source_label, `last_satellite_anchor_age_ms`, `smoothed`, `emitted_at`). UUID stringification in assertions.
- `tests/unit/c5_state/test_smoke.py` — asserts importability of the new C5 public API.
- `tests/unit/c4_pose/test_smoke.py` — removed the `EstimatorOutput` assertion (no longer in C4).
### Modified (contracts)
- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — v1.0.0 → status `active` (was `draft`).
- `_docs/02_document/contracts/shared_config/composition_root_protocol.md` — v1.2.0 → v1.3.0. Adds `state` (`C5StateConfig`) block, `build_state_estimator` factory signature, and the ordering invariant that the state factory MUST be invoked before C4's `build_pose_estimator` (so the returned `ISam2GraphHandle` can be injected).
## Contract changes
| Contract | Change | Compat |
|----------|--------|--------|
| `state_estimator_protocol.md` v1.0.0 | Status flipped from `draft` to `active`. No surface change. | first stable release of the C5 contract |
| `composition_root_protocol.md` v1.3.0 | Additive — new `state` config block + `build_state_estimator` factory + state-ingest-thread binding. Existing surface unchanged. | non-breaking |
| `fc_adapter_protocol.md` v1.0.0 | No version bump. Already references `EstimatorOutput` / `PoseSourceLabel` abstractly; the underlying DTO shape now matches the C5 v1.0.0 contract. | n/a |
`EstimatorOutput` / `EstimatorHealth` were previously owned ad-hoc by `_types/pose.py` (no contract). Moving them to `_types/state.py` is a one-way migration with no external consumers (the surface only crossed C5→C8 internally).
## Test counts
| Suite | Before (B11) | After (B12) | Delta |
|-------|--------------|-------------|-------|
| Total passing | 501 | 521 | +20 |
| Skipped | 2 | 2 | 0 |
| AZ-381 (new) | 0 | 20 | +20 |
| C8 (migration impact) | 100% | 100% | 0 regressions |
Run command: `PYTHONPATH=src pytest tests/ -q``521 passed, 2 skipped in ~21s`.
## Lint / type
- `ruff check src/ tests/` — clean (16 fixable issues auto-corrected mid-batch).
- `ruff format src/ tests/` — clean (5 files reformatted mid-batch).
- `mypy src/gps_denied_onboard/_types/state.py src/gps_denied_onboard/components/c5_state/ src/gps_denied_onboard/runtime_root/state_factory.py` — 0 errors on new code (4 pre-existing errors in `logging/structured.py` + `c13_fdr/writer.py` left untouched per scope-discipline rule).
## Architectural notes
- **Single-writer ingest thread (Invariants 1 + 8)**: `bind_state_ingest_thread` enforces that every `add_*` / `current_estimate` / `smoothed_history` call lands on one thread. The composition-root invariant — that C4 and C5 share the same ingest thread — is satisfied by binding both factories to the same `threading.get_ident()` capture before construction.
- **`ISam2GraphHandle` Protocol lives in `_isam2_handle.py`** (underscore-prefixed module) — this keeps the C4-internal injection target out of C5's public `__all__` while still allowing `ISam2GraphHandleImpl` to be imported by AZ-382 when it lands.
- **Build-flag gate** (`BUILD_STATE_GTSAM_ISAM2=ON|OFF`) reads from `os.environ` at factory call time. When the flag is OFF, the factory raises `StateEstimatorConfigError(f"BUILD_STATE_<variant> is OFF…")` — ADR-002 enforcement, identical to the C1/C2/C3/C4 pattern.
- **`PoseSourceLabel` consolidation**: prior to this batch the source label was emitted as a free-form string (`"sat_anchored"`, `"visual_propagated"`). The contract enum (`SATELLITE_ANCHORED`, `VISUAL_PROPAGATED`, `DEAD_RECKONED`) is now the canonical form; `_outbound_provenance` accepts both during the transition window (tests + early callers may still pass strings) but new code MUST use the enum.
## Migration impact
| File | Before shape | After shape | Notes |
|------|--------------|-------------|-------|
| `_types/pose.py:EstimatorOutput` | `frame_id: int`, `pose_se3: Any`, `extras: dict` | _(removed; lives in `_types/state.py`)_ | `PoseEstimate` retained |
| `_types/state.py:EstimatorOutput` | n/a | `frame_id: UUID`, `position_wgs84: LatLonAlt`, `orientation_world_T_body: Quat`, `velocity_world_mps: tuple[float,float,float]`, `covariance_6x6: npt.NDArray`, `source_label: PoseSourceLabel`, `last_satellite_anchor_age_ms: int`, `smoothed: bool`, `emitted_at: int` | matches contract v1.0.0 |
| `_types/state.py:EstimatorHealth` | n/a | `isam2_state: IsamState`, `keyframe_count: int`, `cov_norm_growing_for_s: float`, `spoof_promotion_blocked: bool` | matches contract |
| `_types/emitted.py:EmittedExternalPosition.source_label` | `str` | `PoseSourceLabel` | strict typing |
| C4 `__init__` | re-exported `EstimatorOutput` | removed | C5 ownership |
## Acceptance evidence
| AC | Test | Status |
|----|------|--------|
| AC-1 Protocol conformance | `test_ac1_protocol_runtime_checkable`, `test_ac1_missing_method_fails_isinstance` | PASS |
| AC-2 DTOs frozen + slots | `test_ac2_estimator_output_frozen_and_slotted`, `test_ac2_estimator_health_frozen_and_slotted` | PASS |
| AC-3 IsamState 4 values | `test_ac3_isam_state_has_four_values` | PASS |
| AC-4 Build flag OFF rejected | `test_ac4_build_flag_off_rejected` | PASS |
| AC-5 Unknown strategy rejected | `test_ac5_unknown_strategy_rejected_at_config`, `test_ac5_unknown_strategy_via_factory` | PASS |
| AC-6 Factory tuple + INFO log | `test_ac6_factory_returns_tuple_and_logs` | PASS |
| AC-7 Thread binding | `test_ac7_second_thread_binding_rejected`, `test_ac7_same_thread_rebinding_idempotent` | PASS |
| AC-8 ISam2GraphHandleImpl skeleton | `test_ac8_handle_is_isam2_graph_handle`, `test_ac8_handle_methods_raise_named_task` | PASS |
| AC-9 Public API re-exports | `test_ac9_public_api_resolves`, `test_ac9_internals_not_in_all` | PASS |
| AC-10 Error hierarchy catchability | `test_ac10_every_error_is_state_estimator_error` | PASS |
| NFR build p99 ≤ 50 ms | `test_nfr_perf_build_under_50ms` | PASS |
## Known forward actions (not in scope this batch)
- AZ-382 (iSAM2 + IncrementalFixedLagSmoother wiring) will replace every `NotImplementedError` body in `_isam2_handle.py` with the real GTSAM body. The `ISam2GraphHandle` Protocol surface is frozen here.
- AZ-385 (source-label state machine + spoof-promotion gate) will publish into `SpoofRecoverySink` (introduced in B11) — wiring is already in place at the runtime root.
- AZ-355 (C4 pose protocol) currently does NOT re-export `ISam2GraphHandle` from C4's `__init__` because the Protocol lives in C5. If a future task needs C4-side type checking against the handle, the import path is `gps_denied_onboard.components.c5_state._isam2_handle.ISam2GraphHandle`.
+1 -1
View File
@@ -8,7 +8,7 @@ status: in_progress
sub_step:
phase: 6
name: implement-tasks
detail: "batch 11 of N committed (AZ-396 source-set switch + AZ-397 qgc telemetry adapter)"
detail: "batch 12 of N committed (AZ-381 c5 state protocol + DTOs + factory + ISam2GraphHandle skeleton + strict EstimatorOutput/EstimatorHealth reshape across C8)"
retry_count: 0
cycle: 1
tracker: jira