azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 15:01:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-490] C5 set_takeoff_origin entrypoint + bounded-delta GPS gate

Browse Source 
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:
Oleksandr Bezdieniezhnykh
2026-05-12 02:53:58 +03:00
parent 72a06edab0
commit 8a83166261
23 changed files with 1640 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/components/c5_state/__init__.py
+2
View File
@@ -24,6 +24,7 @@ from gps_denied_onboard._types.state import (
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import (
EstimatorAlreadyStartedError,
EstimatorDegradedError,
EstimatorFatalError,
StateEstimatorConfigError,
@@ -34,6 +35,7 @@ from gps_denied_onboard.config.schema import register_component_block
__all__ = [
"C5StateConfig",
"EstimatorAlreadyStartedError",
"EstimatorDegradedError",
"EstimatorFatalError",
"EstimatorHealth",
src/gps_denied_onboard/components/c5_state/_source_label_sm.py
+128
View File
@@ -43,18 +43,30 @@ from typing import TYPE_CHECKING, Final, Protocol, runtime_checkable
from gps_denied_onboard._types.fc import GpsStatus, Severity
from gps_denied_onboard._types.state import PoseSourceLabel
from gps_denied_onboard.fdr_client.records import FdrRecord
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import GpsHealth
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.fdr_client.client import FdrClient
__all__ = [
"BOUNDED_DELTA_REJECT",
"BOUNDED_DELTA_SOFT",
"RejectionCallback",
"RejectionSubscription",
"SourceLabelStateMachine",
]
# AZ-490 / Principle #11 third clause: tri-state outcome of the
# bounded-delta gate. Kept as module-level string constants rather
# than additions to ``PoseSourceLabel`` because the public C4/C5
# label enum is stable and a soft-admission outcome doesn't change
# the externally observable pose-provenance contract.
BOUNDED_DELTA_SOFT: Final[str] = "BOUNDED_DELTA_SOFT"
BOUNDED_DELTA_REJECT: Final[str] = "BOUNDED_DELTA_REJECT"
# Subscriber signature — composition root receives
# (reason, severity, statustext) on every reject. ``severity`` is
@@ -140,6 +152,7 @@ class SourceLabelStateMachine:
*,
spoof_promotion_min_stable_s: float,
spoof_promotion_visual_consistency_tol_m: float,
spoof_promotion_bounded_delta_m: float,
fdr_client: FdrClient | None,
producer_id: str = "c5_state",
clock_ns: Callable[[], int] = time.monotonic_ns,
@@ -154,8 +167,14 @@ class SourceLabelStateMachine:
"SourceLabelStateMachine.spoof_promotion_visual_consistency_tol_m "
f"must be > 0; got {spoof_promotion_visual_consistency_tol_m}"
)
if spoof_promotion_bounded_delta_m <= 0.0:
raise ValueError(
"SourceLabelStateMachine.spoof_promotion_bounded_delta_m must be > 0; "
f"got {spoof_promotion_bounded_delta_m}"
)
self._min_stable_ns: int = int(spoof_promotion_min_stable_s * 1_000_000_000)
self._consistency_tol_m: float = spoof_promotion_visual_consistency_tol_m
self._bounded_delta_m: float = spoof_promotion_bounded_delta_m
self._fdr_client: FdrClient | None = fdr_client
self._producer_id: str = producer_id
self._clock_ns: Callable[[], int] = clock_ns
@@ -322,6 +341,115 @@ class SourceLabelStateMachine:
},
)
# ------------------------------------------------------------------
# AZ-490: third clause — bounded-delta GPS gate.
def process_gps_sample(
self,
sample: LatLonAlt,
*,
smoother_estimate: LatLonAlt | None,
now_ns: int | None = None,
) -> str | None:
"""Bounded-delta admission test for an inbound FC GPS sample.
Returns:
* :data:`BOUNDED_DELTA_SOFT` — sample is within the
``spoof_promotion_bounded_delta_m`` ring; the composition
root may attach it as a soft pose-anchor factor.
* :data:`BOUNDED_DELTA_REJECT` — sample is outside the ring;
the composition root MUST drop it; the dwell-time clause
(clause 1 of the spoof-promotion gate) is reset so a
subsequent reanchor must wait the full
``spoof_promotion_min_stable_s`` window.
* ``None`` — no smoother estimate is available yet (cold
start / fallback engaged); the gate is skipped and the
caller decides whether to admit or drop.
AC-15: distance is computed via :meth:`WgsConverter.horizontal_distance_m`,
which routes through ``pyproj``'s ECEF chain (matches Vincenty
within sub-mm at the bounded-delta operating range), so the
haversine-on-equirectangular shortcut is excluded.
"""
if smoother_estimate is None:
return None
ts = now_ns if now_ns is not None else self._clock_ns()
try:
distance_m = WgsConverter.horizontal_distance_m(smoother_estimate, sample)
except Exception as exc:
self._log.error(
"c5.gps_bounded_delta.distance_failed",
extra={
"kind": "c5.gps_bounded_delta.distance_failed",
"kv": {"error": str(exc)},
},
)
return None
threshold_m = self._bounded_delta_m
admitted = distance_m <= threshold_m
kind = (
"c5.gps_bounded_delta.accept" if admitted else "c5.gps_bounded_delta.reject"
)
record = FdrRecord(
schema_version=1,
ts=datetime.now(tz=timezone.utc).isoformat(),
producer_id=self._producer_id,
kind=kind,
payload={
"sample_lat": sample.lat_deg,
"sample_lon": sample.lon_deg,
"smoother_lat": smoother_estimate.lat_deg,
"smoother_lon": smoother_estimate.lon_deg,
"distance_m": distance_m,
"threshold_m": threshold_m,
},
)
if self._fdr_client is not None:
try:
self._fdr_client.enqueue(record)
except Exception as exc:
self._log.warning(
"c5.gps_bounded_delta.fdr_enqueue_failed",
extra={
"kind": "c5.gps_bounded_delta.fdr_enqueue_failed",
"kv": {"reject": not admitted, "error": repr(exc)},
},
)
if admitted:
self._log.info(
"c5.gps_bounded_delta.accept",
extra={
"kind": "c5.gps_bounded_delta.accept",
"kv": {
"distance_m": distance_m,
"threshold_m": threshold_m,
},
},
)
return BOUNDED_DELTA_SOFT
# Reject path: bump the dwell-time clause so the AZ-385 gate
# holds steady — a wildly-off GPS sample is a strong signal
# the FC stream is not (yet) reliable, even when health is
# nominally STABLE_NON_SPOOFED.
with self._lock:
self._gps_health_stable_since_ns = None
self._log.warning(
"c5.gps_bounded_delta.reject",
extra={
"kind": "c5.gps_bounded_delta.reject",
"kv": {
"distance_m": distance_m,
"threshold_m": threshold_m,
"sample_lat": sample.lat_deg,
"sample_lon": sample.lon_deg,
"smoother_lat": smoother_estimate.lat_deg,
"smoother_lon": smoother_estimate.lon_deg,
"ts_ns": ts,
},
},
)
return BOUNDED_DELTA_REJECT
# ------------------------------------------------------------------
# Subscription API.
src/gps_denied_onboard/components/c5_state/config.py
+27
View File
@@ -37,6 +37,15 @@ class C5StateConfig:
in ``STABLE_NON_SPOOFED`` before the spoof-promotion gate opens.
- ``spoof_promotion_visual_consistency_tol_m`` — AC-NEW-8 visual
consistency tolerance on the next anchor.
- ``spoof_promotion_bounded_delta_m`` — AZ-490 / Principle #11
third clause: max horizontal distance (m) between an inbound FC
GPS sample and the smoother's current estimate; samples within
the ring are admitted as ``BOUNDED_DELTA_SOFT``, samples outside
are rejected and counted against the dwell-time clause.
- ``default_takeoff_origin_sigma_horiz_m`` — AZ-490 default
horizontal sigma when ``set_takeoff_origin`` callers omit one.
- ``default_takeoff_origin_sigma_vert_m`` — AZ-490 default
vertical sigma when ``set_takeoff_origin`` callers omit one.
- ``no_estimate_fallback_s`` — AC-5.2 timeout before the
runtime root drops to FC-IMU-only mode.
"""
@@ -45,6 +54,9 @@ class C5StateConfig:
keyframe_window_size: int = 15
spoof_promotion_min_stable_s: float = 10.0
spoof_promotion_visual_consistency_tol_m: float = 30.0
spoof_promotion_bounded_delta_m: float = 200.0
default_takeoff_origin_sigma_horiz_m: float = 5.0
default_takeoff_origin_sigma_vert_m: float = 10.0
no_estimate_fallback_s: float = 3.0
def __post_init__(self) -> None:
@@ -68,6 +80,21 @@ class C5StateConfig:
"C5StateConfig.spoof_promotion_visual_consistency_tol_m must be > 0; "
f"got {self.spoof_promotion_visual_consistency_tol_m}"
)
if self.spoof_promotion_bounded_delta_m <= 0.0:
raise ConfigError(
"C5StateConfig.spoof_promotion_bounded_delta_m must be > 0; "
f"got {self.spoof_promotion_bounded_delta_m}"
)
if self.default_takeoff_origin_sigma_horiz_m <= 0.0:
raise ConfigError(
"C5StateConfig.default_takeoff_origin_sigma_horiz_m must be > 0; "
f"got {self.default_takeoff_origin_sigma_horiz_m}"
)
if self.default_takeoff_origin_sigma_vert_m <= 0.0:
raise ConfigError(
"C5StateConfig.default_takeoff_origin_sigma_vert_m must be > 0; "
f"got {self.default_takeoff_origin_sigma_vert_m}"
)
if self.no_estimate_fallback_s <= 0.0:
raise ConfigError(
"C5StateConfig.no_estimate_fallback_s must be > 0; "
src/gps_denied_onboard/components/c5_state/errors.py
+19
View File
@@ -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.
"""
src/gps_denied_onboard/components/c5_state/eskf_baseline.py
+195 -2
View File
@@ -46,9 +46,11 @@ filter; this module documents the deviation in the
from __future__ import annotations
import math
import time
from collections import deque
from typing import TYPE_CHECKING, Any, Final
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Any, Final, Literal
from uuid import uuid4
import numpy as np
@@ -75,16 +77,18 @@ from gps_denied_onboard.components.c5_state._source_label_sm import (
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import (
EstimatorAlreadyStartedError,
EstimatorDegradedError,
EstimatorFatalError,
StateEstimatorConfigError,
)
from gps_denied_onboard.components.c5_state.interface import StateEstimator
from gps_denied_onboard.fdr_client.records import FdrRecord
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import GpsHealth
from gps_denied_onboard._types.fc import GpsHealth, GpsSample
from gps_denied_onboard._types.nav import ImuWindow
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.vio import VioOutput
@@ -196,11 +200,20 @@ class EskfStateEstimator(StateEstimator):
self._enu_origin: LatLonAlt | None = None
self._history: deque[EstimatorOutput] = deque(maxlen=_HISTORY_DEPTH)
# AZ-490 cold-start ladder. Same semantics as
# ``GtsamIsam2StateEstimator`` — see that class for the
# field-level rationale (idempotency, conflict detection,
# window-closed gate, FC-EKF fallback FDR).
self._takeoff_origin_set: tuple[LatLonAlt, float, float] | None = None
self._origin_source: Literal["manifest", "fc_ekf"] | None = None
self._cold_start_window_closed: bool = False
# AZ-385: source-label SM. Eagerly constructed; composition
# root drives notify_gps_health + subscribe_spoof_rejection.
self._source_label_machine: SourceLabelStateMachine = SourceLabelStateMachine(
spoof_promotion_min_stable_s=block.spoof_promotion_min_stable_s,
spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m,
spoof_promotion_bounded_delta_m=block.spoof_promotion_bounded_delta_m,
fdr_client=fdr_client,
producer_id="c5_state",
)
@@ -278,6 +291,151 @@ class EskfStateEstimator(StateEstimator):
) -> FallbackSubscription:
return self._fallback.subscribe_recovered(callback)
# ------------------------------------------------------------------
# AZ-490: operator-origin cold-start entrypoint.
def set_takeoff_origin(
self,
origin: LatLonAlt,
sigma_horiz_m: float,
sigma_vert_m: float,
) -> None:
"""Seed the ESKF nominal state + position covariance from operator origin.
See :meth:`StateEstimator.set_takeoff_origin` for the full
contract. ESKF impl: sets the ENU origin to ``origin``,
zeros the nominal position (the operator-supplied origin
IS the local-ENU (0,0,0)), and writes the position block of
``_P`` to ``diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)``.
"""
_validate_takeoff_origin_args(origin, sigma_horiz_m, sigma_vert_m)
if self._cold_start_window_closed:
raise EstimatorAlreadyStartedError(
"set_takeoff_origin called after the cold-start window closed; "
"first add_* call sealed the operator-origin entrypoint"
)
if self._takeoff_origin_set is not None:
prev_origin, prev_sh, prev_sv = self._takeoff_origin_set
if prev_origin == origin and prev_sh == sigma_horiz_m and prev_sv == sigma_vert_m:
return # AC-4 — idempotent no-op
raise StateEstimatorConfigError(
"set_takeoff_origin re-called with conflicting args; "
f"previous=(origin={prev_origin!r}, sigma_horiz_m={prev_sh}, "
f"sigma_vert_m={prev_sv}); "
f"new=(origin={origin!r}, sigma_horiz_m={sigma_horiz_m}, "
f"sigma_vert_m={sigma_vert_m})"
)
self._enu_origin = origin
self._nominal_pos = np.zeros(3, dtype=np.float64)
# Position block of the error covariance — explicit overwrite
# (the diagonal stays diagonal here; off-diagonal terms in
# ``_P[0:3, ...]`` were zero at construction and we don't
# want to lose those zeros).
pos_var = np.diag(
np.array(
[sigma_horiz_m**2, sigma_horiz_m**2, sigma_vert_m**2],
dtype=np.float64,
)
)
self._P[_IDX_POS, _IDX_POS] = pos_var
# Symmetrise defensively — the rest of the matrix may carry
# accumulated cross-terms from earlier construction; the
# symmetrise costs nothing and keeps the SPD invariant
# observable on the next read.
self._P = 0.5 * (self._P + self._P.T)
self._takeoff_origin_set = (origin, sigma_horiz_m, sigma_vert_m)
self._origin_source = "manifest"
self._emit_cold_start_origin_set_fdr(
source="manifest",
origin=origin,
sigma_horiz_m=sigma_horiz_m,
sigma_vert_m=sigma_vert_m,
)
self._log.info(
"c5.cold_start_origin.set",
extra={
"kind": "c5.cold_start_origin.set",
"kv": {
"source": "manifest",
"lat_deg": origin.lat_deg,
"lon_deg": origin.lon_deg,
"alt_m": origin.alt_m,
"sigma_horiz_m": sigma_horiz_m,
"sigma_vert_m": sigma_vert_m,
},
},
)
def _close_cold_start_window(self) -> None:
"""ESKF mirror of the iSAM2 helper — see that class for the contract."""
if self._cold_start_window_closed:
return
self._cold_start_window_closed = True
if self._origin_source is None:
self._origin_source = "fc_ekf"
origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
self._emit_cold_start_origin_set_fdr(
source="fc_ekf",
origin=origin,
sigma_horiz_m=self._block.default_takeoff_origin_sigma_horiz_m,
sigma_vert_m=self._block.default_takeoff_origin_sigma_vert_m,
)
def _emit_cold_start_origin_set_fdr(
self,
*,
source: Literal["manifest", "fc_ekf"],
origin: LatLonAlt,
sigma_horiz_m: float,
sigma_vert_m: float,
) -> None:
if self._fdr_client is None:
return
record = FdrRecord(
schema_version=1,
ts=datetime.now(tz=timezone.utc).isoformat(),
producer_id="c5_state",
kind="c5.cold_start_origin.set",
payload={
"source": source,
"lat_deg": origin.lat_deg,
"lon_deg": origin.lon_deg,
"alt_m": origin.alt_m,
"sigma_horiz_m": sigma_horiz_m,
"sigma_vert_m": sigma_vert_m,
},
)
try:
self._fdr_client.enqueue(record)
except Exception as exc:
self._log.warning(
"c5.cold_start_origin.set_fdr_enqueue_failed",
extra={
"kind": "c5.cold_start_origin.set_fdr_enqueue_failed",
"kv": {"source": source, "error": repr(exc)},
},
)
def notify_gps_sample(self, sample: GpsSample, now_ns: int | None = None) -> str | None:
"""ESKF bounded-delta dispatch — mirror of the iSAM2 method."""
machine = self._source_label_machine
if not isinstance(machine, SourceLabelStateMachine):
return None
smoother_latlon: LatLonAlt | None
try:
smoother_latlon = self._enu_pose_to_wgs84()
except Exception:
smoother_latlon = None
return machine.process_gps_sample(
sample.position_wgs84,
smoother_estimate=smoother_latlon,
now_ns=now_ns,
)
# ------------------------------------------------------------------
# Protocol: factor adds.
@@ -290,6 +448,7 @@ class EskfStateEstimator(StateEstimator):
the analogous nominal delta — both are projected to a 6-vector
residual in the previous body frame.
"""
self._close_cold_start_window()
ts_ns = _datetime_to_ns(vio.timestamp)
self._guard_timestamp(ts_ns, source="vio")
curr_pose = _pose_se3_to_array(vio.pose_se3)
@@ -372,6 +531,7 @@ class EskfStateEstimator(StateEstimator):
has no graph to throttle); it integrates every anchor as a
regular measurement.
"""
self._close_cold_start_window()
ts_ns = int(pose.emitted_at)
self._guard_timestamp(ts_ns, source="pose_anchor")
meas_pose = self._pose_estimate_to_matrix(pose)
@@ -415,6 +575,7 @@ class EskfStateEstimator(StateEstimator):
def add_fc_imu(self, imu_window: ImuWindow) -> None:
"""Predict nominal state + propagate covariance over the IMU window."""
self._close_cold_start_window()
self._guard_timestamp(imu_window.ts_end_ns, source="imu_window")
samples = imu_window.samples
if not samples:
@@ -925,6 +1086,38 @@ def _enforce_spd(cov: np.ndarray) -> None:
raise EstimatorFatalError(f"covariance not SPD: {exc}") from exc
def _validate_takeoff_origin_args(
origin: LatLonAlt,
sigma_horiz_m: float,
sigma_vert_m: float,
) -> None:
"""AZ-490 input validation — same rules for both estimator impls."""
if not (
math.isfinite(origin.lat_deg)
and math.isfinite(origin.lon_deg)
and math.isfinite(origin.alt_m)
):
raise StateEstimatorConfigError(
f"set_takeoff_origin: non-finite component in origin {origin!r}"
)
if not (-90.0 <= origin.lat_deg <= 90.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: latitude {origin.lat_deg} outside WGS-84 [-90, 90]"
)
if not (-180.0 <= origin.lon_deg <= 180.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: longitude {origin.lon_deg} outside WGS-84 [-180, 180]"
)
if not (math.isfinite(sigma_horiz_m) and sigma_horiz_m > 0.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: sigma_horiz_m must be positive finite; got {sigma_horiz_m}"
)
if not (math.isfinite(sigma_vert_m) and sigma_vert_m > 0.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: sigma_vert_m must be positive finite; got {sigma_vert_m}"
)
def _with_smoothed_false(out: EstimatorOutput) -> EstimatorOutput:
"""Return a copy of ``out`` with ``smoothed=False``.
src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py
+252 -2
View File
@@ -30,10 +30,11 @@ there.
from __future__ import annotations
import math
import time
from collections import deque
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Any, Final
from typing import TYPE_CHECKING, Any, Final, Literal
from uuid import UUID, uuid4
import gtsam
@@ -66,6 +67,7 @@ from gps_denied_onboard.components.c5_state._source_label_sm import (
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import (
EstimatorAlreadyStartedError,
EstimatorDegradedError,
EstimatorFatalError,
StateEstimatorConfigError,
@@ -76,7 +78,7 @@ from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import GpsHealth
from gps_denied_onboard._types.fc import GpsHealth, GpsSample
from gps_denied_onboard._types.nav import ImuWindow
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.vio import VioOutput
@@ -120,6 +122,13 @@ _COV_NORM_WINDOW_NS: Final[int] = 60 * 1_000_000_000
# state. AZ-385 will tie origin selection to the spoof-promotion gate.
_DEFAULT_ENU_ORIGIN: Final[LatLonAlt] = LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0)
# AZ-490 / ADR-010 default rotation sigma for the operator-origin
# prior factor. Translation sigmas are caller-supplied; the rotation
# component has no operator input, so we use a conservative 5° prior
# (a still-stationary drone with no compass calibration is comfortably
# inside a 5° envelope at the WGS84 frame).
_DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD: Final[float] = math.radians(5.0)
class GtsamIsam2StateEstimator(StateEstimator):
"""Production-default C5 estimator — iSAM2 + ``IncrementalFixedLagSmoother``.
@@ -213,6 +222,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
self._source_label_machine: SourceLabelStateMachine = SourceLabelStateMachine(
spoof_promotion_min_stable_s=block.spoof_promotion_min_stable_s,
spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m,
spoof_promotion_bounded_delta_m=block.spoof_promotion_bounded_delta_m,
fdr_client=fdr_client,
producer_id="c5_state",
)
@@ -248,6 +258,25 @@ class GtsamIsam2StateEstimator(StateEstimator):
producer_id="c5_state",
)
# AZ-490 cold-start ladder ----------------------------------------
# ``_takeoff_origin_set`` records the operator-origin args so
# AC-4 idempotency + AC-5 conflict checks can compare without
# re-deriving from the seeded ``PriorFactorPose3``. None means
# ``set_takeoff_origin`` has not been called yet.
self._takeoff_origin_set: tuple[LatLonAlt, float, float] | None = None
# ``_origin_source`` tracks which cold-start anchor won.
# ``"manifest"`` is set by ``set_takeoff_origin`` (Principle
# #11 primary); ``"fc_ekf"`` is set by the first ``add_*``
# call when no manifest origin was supplied (legacy fallback).
# AC-13 wants exactly one ``c5.cold_start_origin.set`` FDR
# record per estimator; this field gates the emission.
self._origin_source: Literal["manifest", "fc_ekf"] | None = None
# ``_cold_start_window_closed`` flips True on the first
# ``add_*`` call (vio / pose_anchor / fc_imu). After it
# closes, ``set_takeoff_origin`` must raise
# ``EstimatorAlreadyStartedError`` (AC-6).
self._cold_start_window_closed: bool = False
self._log.debug(
"c5.state.isam2_initialised",
extra={
@@ -392,6 +421,224 @@ class GtsamIsam2StateEstimator(StateEstimator):
self._next_key_counter += 1
return new_key
# ------------------------------------------------------------------
# AZ-490: operator-origin cold-start entrypoint.
def set_takeoff_origin(
self,
origin: LatLonAlt,
sigma_horiz_m: float,
sigma_vert_m: float,
) -> None:
"""Seed the cold-start prior with an operator-supplied origin (AZ-490).
See :meth:`StateEstimator.set_takeoff_origin` for the full
contract. This impl sets the local ENU origin to ``origin``,
attaches one ``PriorFactorPose3`` at ``Pose3.Identity()`` to
the next pose key, drives a single ``handle.update``, and
emits one ``c5.cold_start_origin.set`` FDR record.
"""
self._validate_takeoff_origin_args(origin, sigma_horiz_m, sigma_vert_m)
if self._cold_start_window_closed:
raise EstimatorAlreadyStartedError(
"set_takeoff_origin called after the cold-start window closed; "
"first add_* call sealed the operator-origin entrypoint"
)
if self._takeoff_origin_set is not None:
prev_origin, prev_sh, prev_sv = self._takeoff_origin_set
if prev_origin == origin and prev_sh == sigma_horiz_m and prev_sv == sigma_vert_m:
return # AC-4 — idempotent no-op
raise StateEstimatorConfigError(
"set_takeoff_origin re-called with conflicting args; "
f"previous=(origin={prev_origin!r}, sigma_horiz_m={prev_sh}, "
f"sigma_vert_m={prev_sv}); "
f"new=(origin={origin!r}, sigma_horiz_m={sigma_horiz_m}, "
f"sigma_vert_m={sigma_vert_m})"
)
handle = self._require_handle()
self._enu_origin = origin
prior_pose = gtsam.Pose3() # Identity at ENU origin
prior_key = gtsam.symbol("x", self._next_key_counter)
self._next_key_counter += 1
sigmas = np.array(
[
_DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD,
_DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD,
_DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD,
sigma_horiz_m,
sigma_horiz_m,
sigma_vert_m,
],
dtype=np.float64,
)
noise = gtsam.noiseModel.Diagonal.Sigmas(sigmas)
factor = gtsam.PriorFactorPose3(prior_key, prior_pose, noise)
# AC-6 / Invariant 11a: do NOT advance ``_last_added_ts_ns`` —
# this is a pre-takeoff seed, not a measurement; the first
# subsequent ``add_*`` call still sees the unguarded baseline.
ts_ns = time.monotonic_ns()
try:
handle.add_factor(factor)
self._values.insert(prior_key, prior_pose)
timestamps = _make_timestamp_map([prior_key], ts_ns)
handle.update(self._graph, self._values, timestamps)
except (EstimatorDegradedError, EstimatorFatalError):
raise
except Exception as exc:
self._log.error(
"c5.state.set_takeoff_origin_failed",
extra={
"kind": "c5.state.set_takeoff_origin_failed",
"kv": {"error": str(exc)},
},
)
raise EstimatorDegradedError(f"set_takeoff_origin failed: {exc}") from exc
self._reset_staging()
self._record_committed_pose_key(prior_key)
self._takeoff_origin_set = (origin, sigma_horiz_m, sigma_vert_m)
self._origin_source = "manifest"
self._emit_cold_start_origin_set_fdr(
source="manifest",
origin=origin,
sigma_horiz_m=sigma_horiz_m,
sigma_vert_m=sigma_vert_m,
)
self._log.info(
"c5.cold_start_origin.set",
extra={
"kind": "c5.cold_start_origin.set",
"kv": {
"source": "manifest",
"lat_deg": origin.lat_deg,
"lon_deg": origin.lon_deg,
"alt_m": origin.alt_m,
"sigma_horiz_m": sigma_horiz_m,
"sigma_vert_m": sigma_vert_m,
},
},
)
@staticmethod
def _validate_takeoff_origin_args(
origin: LatLonAlt,
sigma_horiz_m: float,
sigma_vert_m: float,
) -> None:
if not (
math.isfinite(origin.lat_deg)
and math.isfinite(origin.lon_deg)
and math.isfinite(origin.alt_m)
):
raise StateEstimatorConfigError(
f"set_takeoff_origin: non-finite component in origin {origin!r}"
)
if not (-90.0 <= origin.lat_deg <= 90.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: latitude {origin.lat_deg} outside WGS-84 [-90, 90]"
)
if not (-180.0 <= origin.lon_deg <= 180.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: longitude {origin.lon_deg} outside WGS-84 [-180, 180]"
)
if not (math.isfinite(sigma_horiz_m) and sigma_horiz_m > 0.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: sigma_horiz_m must be positive finite; got {sigma_horiz_m}"
)
if not (math.isfinite(sigma_vert_m) and sigma_vert_m > 0.0):
raise StateEstimatorConfigError(
f"set_takeoff_origin: sigma_vert_m must be positive finite; got {sigma_vert_m}"
)
def _close_cold_start_window(self) -> None:
"""Mark the cold-start window closed on the first ``add_*`` call.
Idempotent — only the first invocation flips the flag and
emits the legacy-fallback FDR record (AC-13). Subsequent
calls are no-ops.
"""
if self._cold_start_window_closed:
return
self._cold_start_window_closed = True
if self._origin_source is None:
self._origin_source = "fc_ekf"
origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
self._emit_cold_start_origin_set_fdr(
source="fc_ekf",
origin=origin,
sigma_horiz_m=self._block.default_takeoff_origin_sigma_horiz_m,
sigma_vert_m=self._block.default_takeoff_origin_sigma_vert_m,
)
def _emit_cold_start_origin_set_fdr(
self,
*,
source: Literal["manifest", "fc_ekf"],
origin: LatLonAlt,
sigma_horiz_m: float,
sigma_vert_m: float,
) -> None:
if self._fdr_client is None:
return
record = FdrRecord(
schema_version=1,
ts=datetime.now(tz=timezone.utc).isoformat(),
producer_id="c5_state",
kind="c5.cold_start_origin.set",
payload={
"source": source,
"lat_deg": origin.lat_deg,
"lon_deg": origin.lon_deg,
"alt_m": origin.alt_m,
"sigma_horiz_m": sigma_horiz_m,
"sigma_vert_m": sigma_vert_m,
},
)
try:
self._fdr_client.enqueue(record)
except Exception as exc:
self._log.warning(
"c5.cold_start_origin.set_fdr_enqueue_failed",
extra={
"kind": "c5.cold_start_origin.set_fdr_enqueue_failed",
"kv": {"source": source, "error": repr(exc)},
},
)
def notify_gps_sample(self, sample: GpsSample, now_ns: int | None = None) -> str | None:
"""Bounded-delta gate dispatch for an inbound FC GPS sample (AZ-490).
Looks up the smoother's current latlon (best-effort — returns
``None`` if no committed pose is available yet) and delegates
to :meth:`SourceLabelStateMachine.process_gps_sample`. The
return value mirrors that method's tri-state result
(``"BOUNDED_DELTA_SOFT"`` / ``"REJECT"`` / ``None``); the
composition root uses it to decide whether to enqueue the
sample as a soft factor.
"""
machine = self._source_label_machine
if not isinstance(machine, SourceLabelStateMachine):
return None
smoother_latlon: LatLonAlt | None
if self._last_committed_pose_key is None:
smoother_latlon = None
else:
try:
pose = self._pose_at_key(self._last_committed_pose_key)
smoother_latlon = self._enu_pose_to_wgs84(pose)
except EstimatorFatalError:
smoother_latlon = None
return machine.process_gps_sample(
sample.position_wgs84,
smoother_estimate=smoother_latlon,
now_ns=now_ns,
)
# ------------------------------------------------------------------
# AZ-383: factor-add bodies.
@@ -405,6 +652,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
single ``handle.update()`` per AC-7.
"""
handle = self._require_handle()
self._close_cold_start_window()
ts_ns = _datetime_to_ns(vio.timestamp)
self._guard_timestamp(ts_ns, source="vio")
@@ -476,6 +724,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
gate (AZ-385) and ``last_anchor_age_ms`` see a recent anchor.
"""
handle = self._require_handle()
self._close_cold_start_window()
ts_ns = int(pose.emitted_at)
self._guard_timestamp(ts_ns, source="pose_anchor")
@@ -560,6 +809,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
owns the per-window factor add.
"""
handle = self._require_handle()
self._close_cold_start_window()
self._guard_timestamp(imu_window.ts_end_ns, source="imu_window")
try:
src/gps_denied_onboard/components/c5_state/interface.py
+28 -1
View File
@@ -21,7 +21,7 @@ from typing import TYPE_CHECKING, Protocol, runtime_checkable
if TYPE_CHECKING:
from gps_denied_onboard._types.nav import ImuWindow
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.pose import LatLonAlt, PoseEstimate
from gps_denied_onboard._types.state import (
EstimatorHealth,
EstimatorOutput,
@@ -41,6 +41,33 @@ class StateEstimator(Protocol):
impls must implement every method.
"""
def set_takeoff_origin(
self,
origin: LatLonAlt,
sigma_horiz_m: float,
sigma_vert_m: float,
) -> None:
"""Seed the cold-start prior with an operator-supplied origin (AZ-490 / ADR-010).
Pre-takeoff entrypoint. Called by the composition root during
F2 (Takeoff load) when the C10 ManifestVerifier reports a
valid ``flight.takeoff_origin``. With this prior set, the
FC-EKF first-frame cold-start path becomes the secondary
fallback (Invariant 11).
Contract:
* Idempotent if called twice with byte-identical args (no-op).
* Raises :class:`StateEstimatorConfigError` if called twice
with different args, on negative / non-finite sigmas, or on
a ``LatLonAlt`` outside WGS-84 bounds.
* Raises :class:`EstimatorAlreadyStartedError` if called
AFTER the first ``add_vio`` / ``add_pose_anchor`` /
``add_fc_imu`` (cold-start window closed).
* Emits one ``c5.cold_start_origin.set`` FDR record with
``source="manifest"``.
"""
def add_vio(self, vio: VioOutput) -> None:
"""Add a VIO output as a relative-pose factor to the iSAM2 graph."""