mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 06:31:14 +00:00
Oleksandr Bezdieniezhnykh
8a83166261
Add operator warm-start path to C5 StateEstimator Protocol and both
implementations (GtsamIsam2StateEstimator, EskfStateEstimator), plus
the third clause of the AZ-385 spoof-promotion gate.
- StateEstimator Protocol: set_takeoff_origin(origin, sigma_horiz_m,
sigma_vert_m) -> None.
- iSAM2: PriorFactorPose3 at origin with diagonal sigmas, single
isam2.update().
- ESKF: zero _nominal_pos, overwrite _P position block with sigma**2.
- SourceLabelStateMachine.process_gps_sample bounded-delta clause:
WgsConverter.horizontal_distance_m vs smoother estimate; reject
resets the dwell-time counter so AZ-385 cannot re-promote off bad
GPS.
- New EstimatorAlreadyStartedError (StateEstimatorConfigError
subclass) on late call after first add_*.
- C5StateConfig: spoof_promotion_bounded_delta_m=200,
default_takeoff_origin_sigma_horiz_m=5,
default_takeoff_origin_sigma_vert_m=10.
- New GpsSample DTO + WgsConverter.horizontal_distance_m helper.
- 4 new FDR kinds (cold_start_origin.{set,unavailable},
gps_bounded_delta.{accept,reject}) registered in AZ-272 schema.
- 33 new unit tests cover AC-1..AC-15; full repo 750 passed / 2
skipped (pre-existing CI tooling skips).
Docs synced: protocol contract, C5 component description,
architecture, glossary, system-flows, C10 provisioning description.
Co-authored-by: Cursor <cursoragent@cursor.com>
75 lines
2.7 KiB
Python
75 lines
2.7 KiB
Python
"""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__ = [
|
|
"EstimatorAlreadyStartedError",
|
|
"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.
|
|
|
|
AZ-490: also raised by :meth:`StateEstimator.set_takeoff_origin`
|
|
when the supplied ``LatLonAlt`` is outside WGS-84 bounds, when
|
|
either sigma is non-positive / non-finite, or when the entrypoint
|
|
is called twice with conflicting arguments before the first
|
|
measurement.
|
|
"""
|
|
|
|
|
|
class EstimatorAlreadyStartedError(StateEstimatorConfigError):
|
|
"""``set_takeoff_origin`` called after the estimator left the INIT state.
|
|
|
|
AZ-490 / Contract Invariant 11a: the operator-origin entrypoint is
|
|
valid only before the first ``add_*`` call. Once any factor has
|
|
been added (i.e. the smoother is past INIT), seeding a new prior
|
|
would silently corrupt the running estimate. ``IS-A`` of
|
|
:class:`StateEstimatorConfigError` so existing
|
|
``except StateEstimatorConfigError`` callers also catch this.
|
|
"""
|