[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
@@ -0,0 +1,55 @@
"""C5 ``StateEstimator`` error hierarchy — AZ-381.
Every C5-emitted exception inherits :class:`StateEstimatorError`
(AC-10) so callers can write a single ``except`` against the whole
component surface. Composition-root failures use
:class:`StateEstimatorConfigError`; runtime failures split into
``Degraded`` (recoverable; emit a degraded estimate + log) vs
``Fatal`` (unrecoverable; trigger the AC-5.2 IMU-only fallback path
in C8).
"""
from __future__ import annotations
__all__ = [
"EstimatorDegradedError",
"EstimatorFatalError",
"StateEstimatorConfigError",
"StateEstimatorError",
]
class StateEstimatorError(Exception):
"""Base class for every C5-emitted exception (AC-10)."""
class EstimatorDegradedError(StateEstimatorError):
"""Recoverable runtime degradation.
Examples: out-of-order ``add_*`` call (Invariant 2), failed factor
add against the graph (R05 mitigation surfaces via this), poor
convergence detected post-update. The estimator continues to
produce outputs but the next ``current_estimate()`` may carry a
degraded ``EstimatorHealth.isam2_state``.
"""
class EstimatorFatalError(StateEstimatorError):
"""Unrecoverable numerical failure.
Raised when iSAM2 / Marginals / the smoother enter a state from
which the run cannot continue: non-SPD posterior covariance after
update, NaN propagation, GTSAM exception bubbling. Triggers the
AC-5.2 path in C8 (IMU-only fallback) and the source-label state
machine transitions to ``DEAD_RECKONED``.
"""
class StateEstimatorConfigError(StateEstimatorError):
"""Composition-time configuration error.
Raised by :func:`build_state_estimator` when the requested
strategy is not registered (per ADR-002 build flag gating), when
the config schema fails validation, or when the runtime root
cannot wire the iSAM2 graph handle into C4.
"""