mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 11:21:12 +00:00
[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:
@@ -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.
|
||||
"""
|
||||
Reference in New Issue
Block a user