mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 10:01:12 +00:00
[AZ-490] C5 set_takeoff_origin entrypoint + bounded-delta GPS gate
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>
This commit is contained in:
@@ -12,6 +12,7 @@ in C8).
|
||||
from __future__ import annotations
|
||||
|
||||
__all__ = [
|
||||
"EstimatorAlreadyStartedError",
|
||||
"EstimatorDegradedError",
|
||||
"EstimatorFatalError",
|
||||
"StateEstimatorConfigError",
|
||||
@@ -52,4 +53,22 @@ class StateEstimatorConfigError(StateEstimatorError):
|
||||
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.
|
||||
"""
|
||||
|
||||
Reference in New Issue
Block a user