diff --git a/_docs/02_document/architecture.md b/_docs/02_document/architecture.md index bc28363..fb53af5 100644 --- a/_docs/02_document/architecture.md +++ b/_docs/02_document/architecture.md @@ -40,7 +40,7 @@ The system is a **Jetson Orin Nano Super-hosted onboard companion** that deliver 9. **Two execution tiers** (Tier-1 workstation Docker = fast/cheap; Tier-2 Jetson hardware = AC-bound) appear in the deployment plan and CI matrix per finding F6. 10. **Camera intrinsics and full-altitude footage are calibration prerequisites**, not implementation gaps. Production accuracy claims are gated on D-PROJ-1 closure (hybrid factory + checkerboard refinement). Test fixtures use `adti26` calibration sourced from public/factory references. 11. **Spoofed GPS never re-enters the estimator** unless the FC GPS report passes a three-part gate (AC-NEW-8 + AZ-490 follow-up): (a) FC GPS health stable + non-spoofed for ≥ 10 s, (b) a visual/satellite consistency check has succeeded on the next anchor frame, AND (c) the FC's reported position is within ≤ 200 m of the companion's last emitted `PoseEstimate`. The third clause is the **mid-flight bounded-delta gate** — even a "stable, non-spoofed" GPS frame is rejected if it disagrees with the companion's posterior by more than the configurable budget. Real GPS that passes the gate is fused via `add_pose_anchor` with the FC's covariance (treated as one more anchor source, never overriding the visual pipeline without the gate). -14. **Operator-planned mission is the primary cold-start trust anchor**, not the FC EKF (AZ-490 follow-up). The operator authors the route in the parent-suite **Mission Planner UI** (`suite/ui`), the route persists in the parent-suite **`flights` REST service** (`suite/flights`), and C12 (operator tooling) reads the `Flight` from that service to: (a) derive the cache bbox as the envelope of the waypoint lat/lon plus a configurable buffer, (b) extract the first-ordered waypoint as the **takeoff origin** (lat / lon / alt), and (c) bake the takeoff origin into the C10 Manifest so the airborne C5 can warm-start from it via `set_takeoff_origin(origin, sigma_m)` **before** any FC IMU / VIO sample arrives. This unblocks the GPS-jammed-at-takeoff scenario the FC-EKF-only cold-start path (AZ-419 today) cannot handle. The FC EKF's last valid GPS becomes a **secondary** cold-start input — used only when the operator origin is missing from the Manifest OR when the FC EKF reading passes the same bounded-delta consistency check against the operator origin. +14. **Operator-planned mission is the primary cold-start trust anchor**, not the FC EKF (AZ-490 follow-up). The operator authors the route in the parent-suite **Mission Planner UI** (`suite/ui`), the route persists in the parent-suite **`flights` REST service** (`suite/flights`), and C12 (operator tooling) reads the `Flight` from that service to: (a) derive the cache bbox as the envelope of the waypoint lat/lon plus a configurable buffer, (b) extract the first-ordered waypoint as the **takeoff origin** (lat / lon / alt), and (c) bake the takeoff origin into the C10 Manifest so the airborne C5 can warm-start from it via `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` **before** any FC IMU / VIO sample arrives. This unblocks the GPS-jammed-at-takeoff scenario the FC-EKF-only cold-start path (AZ-419 today) cannot handle. The FC EKF's last valid GPS becomes a **secondary** cold-start input — used only when the operator origin is missing from the Manifest OR when the FC EKF reading passes the same bounded-delta consistency check against the operator origin. 12. **AC-4.5 is internal smoothing only.** GTSAM iSAM2 retroactively refines past keyframes onboard and emits the corrected current frame; the FC log is forward-time only — neither ArduPilot nor iNav supports FC-side retroactive correction (Mode B Fact #107). 13. **Interface-first components with constructor-injected dependencies.** Every component is **defined as an interface (Python `Protocol` or `ABC`) before any concrete implementation exists**, lives in its **own folder under `src/components//`**, and is wired together via **constructor injection** at a single composition root. Components never reach out to a global registry, a singleton, or `import` a sibling component's concrete class directly — they receive their collaborators as `__init__` arguments typed against the sibling's interface. Multiple interchangeable implementations of the same interface MUST be supported by design (e.g., C1 has three `VioStrategy` implementations; C2 has UltraVPR + MegaLoc + MixVPR + … behind a single `VprStrategy`; C8 has two FC-adapter implementations behind a single `FcAdapter`). Selection happens once, at startup, by config; the composition root resolves config → concrete implementation → wires the graph; the rest of the runtime sees only interfaces. **Side benefit (NOTE)**: this design also gives the project **packaging optionality** — different combinations of `BUILD_*` flags can produce binaries tailored to specific deployment targets, customer bundles, or (if/when relevant later) end-product licensing strategies, **without any source-level change in application code**. That optionality is a *consequence* of the interface-first design, not a driver — the architectural decisions in this document are made on technical grounds; component licenses do not influence them. See ADR-002 § Consequences and ADR-009. @@ -207,7 +207,7 @@ source repo | `SectorClassification` | `active_conflict | stable_rear` per area, drives freshness threshold | C12 (operator-set) → C6, C10 | | `Flight` | Operator-planned mission: ordered `Waypoint` list + metadata, persisted in the parent-suite `flights` REST service. Read by C12 via `FlightsApiClient`; never reached from the airborne companion | External (`suite/flights`) → C12 | | `Waypoint` | Ordered `(lat, lon, alt, objective, source)` entry inside a `Flight`. C12 envelopes waypoint lat/lon → bbox; first-ordered waypoint → takeoff origin | External (`suite/flights`) → C12 | -| `TakeoffOrigin` | `LatLonAlt` carried in the C10 Manifest; baked in by C12 at build time from `Flight.waypoints[0]`; consumed at boot by C5 via `set_takeoff_origin(origin, sigma_m)` (AZ-490) | C12 → C10 Manifest → C5 | +| `TakeoffOrigin` | `LatLonAlt` carried in the C10 Manifest; baked in by C12 at build time from `Flight.waypoints[0]`; consumed at boot by C5 via `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` (AZ-490) | C12 → C10 Manifest → C5 | **Key relationships**: @@ -619,7 +619,7 @@ This decision is made on **technical grounds only**. Component licenses (BSD/Apa 1. **`Flight` is read pre-flight, not in-flight.** C12 (the operator-side tool, separate binary from the airborne companion — per ADR-002) calls the parent-suite `flights` REST service via a typed client (AZ-489 `FlightsApiClient`) when the operator runs `gps-denied-cli build-cache --flight-id `. An offline path (`--flight-file `) reads the same DTO shape from a JSON export so the workflow survives operator workstations that have no path to the flights service. The companion binary **never** depends on the flights service at runtime (Principle #9 — denied-environment operation). 2. **C12 derives bbox + takeoff origin from the `Flight`.** The bbox is the envelope of waypoint lat/lon plus a configurable buffer (default 1 km, AZ-489 AC-3). The takeoff origin is `Flight.waypoints[0].(lat, lon, alt)` — the operator's authored launch point. 3. **Both fields are baked into the C10 Manifest.** `BuildRequest` and `Manifest` carry `takeoff_origin: LatLonAlt | None` (AZ-323 / AZ-325 / AZ-324 amendments). The hash that drives D-C10-1 idempotence includes `takeoff_origin`, so a re-plan of the route produces a new cache identity and the verifier (AZ-324) rejects a mismatched cache at boot. -4. **C5 consumes the origin before any sensor sample.** The companion's composition root reads `takeoff_origin` from the cache manifest at boot and invokes `set_takeoff_origin(origin, sigma_m)` on the active `StateEstimator` (AZ-490) **before** the first `add_vio` / `add_fc_imu` call. Both `GtsamIsam2StateEstimator` and `EskfStateEstimator` accept the origin as a Bayesian prior — iSAM2 attaches a `PriorFactorPose3` on the initial pose key with covariance derived from `sigma_m` (default 50 m horizontal, 100 m vertical); ESKF seeds the nominal position and writes the position block of the error covariance to match `sigma_m^2`. +4. **C5 consumes the origin before any sensor sample.** The companion's composition root reads `takeoff_origin` from the cache manifest at boot and invokes `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` on the active `StateEstimator` (AZ-490) **before** the first `add_vio` / `add_fc_imu` call. Both `GtsamIsam2StateEstimator` and `EskfStateEstimator` accept the origin as a Bayesian prior — iSAM2 attaches a `PriorFactorPose3` at `Pose3.Identity()` (the operator origin BECOMES the local-ENU (0,0,0) anchor) with diagonal sigmas `[5°, 5°, 5°, sigma_horiz_m, sigma_horiz_m, sigma_vert_m]`; ESKF seeds the nominal position to (0,0,0) and writes the position block of the error covariance to `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`. Defaults are `sigma_horiz_m = 5.0 m`, `sigma_vert_m = 10.0 m` from `C5StateConfig`. 5. **FC GPS is a secondary, gated input.** If the FC EKF later produces a GPS reading (in-flight or at takeoff), it is fused through the existing `add_pose_anchor` machinery only after passing the three-part gate of Principle #11 — **including the ≤ 200 m bounded-delta check against the companion's last emitted `PoseEstimate`**. Real GPS that passes the gate is one more measurement, never an override. 6. **Failure modes.** If the Manifest has no `takeoff_origin` AND the FC EKF has no usable GPS at takeoff, C5 stays in `INITIALIZING` and the FC adapter (C8) emits a non-fused source label; the FT-P-11 takeoff-abort policy (AZ-419 amended) applies. If the Manifest has `takeoff_origin` AND the FC EKF GPS is wildly inconsistent with it at takeoff (e.g., > 200 m), the operator origin wins and the FC GPS is logged as suspect — this is the GPS-spoofed-at-takeoff case and is the entire point of this ADR. @@ -634,7 +634,7 @@ This decision is made on **technical grounds only**. Component licenses (BSD/Apa - AZ-419 (FT-P-11) is amended: the primary cold-start path is operator-origin-from-manifest; FC-EKF-GPS is the fallback path with its own sub-AC. - C10 contracts gain a `takeoff_origin` field in `BuildRequest`, `Manifest`, and the verifier's validation set (AZ-323 / AZ-325 / AZ-324). Contract version bumps to v1.1.0. -- C5 gains a `set_takeoff_origin(origin, sigma_m)` method on the `StateEstimator` protocol (AZ-490). Protocol contract version bumps to v1.1.0. +- C5 gains a `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` method on the `StateEstimator` protocol (AZ-490). Protocol contract version bumps to v1.1.0. - C12 gains the `FlightsApiClient` boundary + offline `--flight-file` path (AZ-489). - Principle #11 (the spoofed-GPS gate) is extended with the bounded-delta clause; the gate now serves both takeoff and mid-flight. - The companion binary's network surface is unchanged — only C12 (operator-side, separate binary) talks to the flights service. \ No newline at end of file diff --git a/_docs/02_document/components/07_c5_state/description.md b/_docs/02_document/components/07_c5_state/description.md index 1e4c32d..4d20e77 100644 --- a/_docs/02_document/components/07_c5_state/description.md +++ b/_docs/02_document/components/07_c5_state/description.md @@ -22,7 +22,7 @@ | Method | Input | Output | Async | Error Types | |--------|-------|--------|-------|-------------| -| `set_takeoff_origin` (AZ-490, ADR-010) | `origin: LatLonAlt, sigma_m: float` | `None` | No | `EstimatorConfigError`, `EstimatorFatalError` | +| `set_takeoff_origin` (AZ-490, ADR-010) | `origin: LatLonAlt, sigma_horiz_m: float, sigma_vert_m: float` | `None` | No | `StateEstimatorConfigError`, `EstimatorAlreadyStartedError` | | `add_vio` | `VioOutput` | `None` | No | `EstimatorDegradedError`, `EstimatorFatalError` | | `add_pose_anchor` | `PoseEstimate` | `None` | No | `EstimatorDegradedError`, `EstimatorFatalError` | | `add_fc_imu` | `ImuWindow` | `None` | No | `EstimatorDegradedError` | @@ -77,7 +77,7 @@ C5 is bounded by design — no unbounded growth. **State Management**: - iSAM2 graph + Values + Marginals lifecycle for the flight. -- Cold-start ladder (ADR-010, AZ-490): `set_takeoff_origin(origin, sigma_m)` MUST be invoked before any `add_vio` / `add_fc_imu` / `add_pose_anchor` call. iSAM2 attaches a `PriorFactorPose3` on the initial pose key with covariance derived from `sigma_m` (default 50 m horizontal, 100 m vertical); ESKF seeds the nominal position and writes the position-block of the error covariance to `sigma_m^2`. The method is idempotent within `INIT` state (re-invocation overwrites the prior); once the estimator transitions to `TRACKING`, further calls raise `EstimatorConfigError`. +- Cold-start ladder (ADR-010, AZ-490): `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` MUST be invoked before any `add_vio` / `add_fc_imu` / `add_pose_anchor` call. The cold-start window closes on the first `add_*` call. iSAM2 attaches a `PriorFactorPose3` at `Pose3.Identity()` (operator origin BECOMES local-ENU (0,0,0)) with diagonal sigmas `[5°, 5°, 5°, sigma_horiz_m, sigma_horiz_m, sigma_vert_m]`; ESKF seeds the nominal position to (0,0,0) and writes the position-block of the error covariance to `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`. The method is **strictly idempotent on identical args** — re-invocation with byte-equal `(origin, sigma_horiz_m, sigma_vert_m)` is a no-op; re-invocation with **different** args raises `StateEstimatorConfigError`. Once the cold-start window closes, further calls raise `EstimatorAlreadyStartedError` (subclass of `StateEstimatorConfigError`). Defaults `default_takeoff_origin_sigma_horiz_m = 5.0`, `default_takeoff_origin_sigma_vert_m = 10.0` live in `C5StateConfig`. - Source-label state machine: tracks the AC-NEW-2 / AC-NEW-8 spoofing-promotion gate (≥10 s + visual consistency check + ≤ 200 m bounded-delta before re-promoting a previously-spoofed FC GPS source). - Last-anchor-age timer for AC-1.3 binning. @@ -90,7 +90,7 @@ C5 is bounded by design — no unbounded growth. | Eigen | matches GTSAM | Lie-algebra math | **Error Handling Strategy**: -- `EstimatorConfigError`: `set_takeoff_origin` called after `TRACKING` state, OR called with a malformed `LatLonAlt` / non-positive `sigma_m`. Caller must surface to operator; takeoff blocked. +- `StateEstimatorConfigError`: `set_takeoff_origin` called with a malformed `LatLonAlt` (out of WGS-84 bounds / non-finite) OR with non-positive / non-finite sigmas, OR re-called inside the cold-start window with conflicting args. `EstimatorAlreadyStartedError` (a `StateEstimatorConfigError` subclass): `set_takeoff_origin` called after the first `add_*` call sealed the cold-start window. Caller must surface to operator; takeoff blocked. - `EstimatorDegradedError`: factor add yielded poor convergence; covariance inflated; emit `EstimatorOutput` with degraded label. - `EstimatorFatalError`: iSAM2 numerical failure, KEYFRAME_LIMIT exceeded, etc.; emit no `EstimatorOutput` for this tick. AC-5.2 fallback (3 s no estimate → FC IMU-only) applies. - Spoof-promotion gate (Principle #11 amended, AZ-385 + AZ-490 follow-up): never re-introduce a previously-spoofed FC GPS source until ALL THREE hold — (i) FC `gps_health == STABLE_NON_SPOOFED` for ≥ 10 s, (ii) the next satellite-anchored frame agrees with the FC GPS within a configurable tolerance, AND (iii) the FC's reported position is within ≤ 200 m of the companion's last emitted `PoseEstimate`. The same gate is applied at takeoff when a Manifest `takeoff_origin` is present: an FC GPS reading that disagrees with the operator origin by > 200 m is logged as suspect and the operator origin wins. Document every reject in FDR + GCS STATUSTEXT. diff --git a/_docs/02_document/components/11_c10_provisioning/description.md b/_docs/02_document/components/11_c10_provisioning/description.md index 57ba531..84d76f5 100644 --- a/_docs/02_document/components/11_c10_provisioning/description.md +++ b/_docs/02_document/components/11_c10_provisioning/description.md @@ -2,7 +2,7 @@ ## 1. High-Level Overview -**Purpose**: build the **model-derived** pre-flight cache artifacts on top of an already-populated tile store, and verify them at takeoff. After C11 `TileDownloader` has fetched tiles into C6, C10 orchestrates: compile/deserialize TensorRT engines via C7 → batch each tile through C2's backbone for descriptors → atomically write FAISS HNSW index with SHA-256 sidecars (D-C10-3) → write Manifest with hash of (model + calibration + corpus + sector_class **+ takeoff_origin**) for D-C10-1 idempotence. The `takeoff_origin` is supplied by C12 (derived from `Flight.waypoints[0]` via the `FlightsApiClient`, ADR-010 + AZ-489); C10 treats it as one more identity field and bakes it into both the Manifest body and the manifest-hash. At F2 takeoff load, run `verify_manifest` (D-C10-3 SHA-256 content-hash gate) before allowing the system to arm; the verifier also surfaces `takeoff_origin` so the companion's composition root can pass it to `C5.set_takeoff_origin(origin, sigma_m)` before any sensor sample (AZ-490). +**Purpose**: build the **model-derived** pre-flight cache artifacts on top of an already-populated tile store, and verify them at takeoff. After C11 `TileDownloader` has fetched tiles into C6, C10 orchestrates: compile/deserialize TensorRT engines via C7 → batch each tile through C2's backbone for descriptors → atomically write FAISS HNSW index with SHA-256 sidecars (D-C10-3) → write Manifest with hash of (model + calibration + corpus + sector_class **+ takeoff_origin**) for D-C10-1 idempotence. The `takeoff_origin` is supplied by C12 (derived from `Flight.waypoints[0]` via the `FlightsApiClient`, ADR-010 + AZ-489); C10 treats it as one more identity field and bakes it into both the Manifest body and the manifest-hash. At F2 takeoff load, run `verify_manifest` (D-C10-3 SHA-256 content-hash gate) before allowing the system to arm; the verifier also surfaces `takeoff_origin` so the companion's composition root can pass it to `C5.set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` before any sensor sample (AZ-490). **C10 does NOT touch `satellite-provider`.** Tile I/O — both download (F1 inbound) and post-landing upload (F10) — lives in C11 (Tile Manager). C10 reads tiles from C6, writes engines + descriptors + manifest to filesystem and Postgres. The split is operational: C11 carries the operator-side network identity (TLS API key for download, per-flight signing key for upload) and the airborne-exclusion property (ADR-004); C10 carries the model identity and the takeoff-load verifier — neither of which need to leave the workstation/companion enclave at runtime. diff --git a/_docs/02_document/contracts/c5_state/state_estimator_protocol.md b/_docs/02_document/contracts/c5_state/state_estimator_protocol.md index b1f6f95..60e1cc8 100644 --- a/_docs/02_document/contracts/c5_state/state_estimator_protocol.md +++ b/_docs/02_document/contracts/c5_state/state_estimator_protocol.md @@ -24,7 +24,12 @@ The shared `ImuPreintegrator` (AZ-276), `SE3Utils` (AZ-277), and `WgsConverter` @runtime_checkable class StateEstimator(Protocol): # AZ-490 / ADR-010: operator-provided warm-start. MUST be called before any add_*. - def set_takeoff_origin(self, origin: LatLonAlt, sigma_m: float) -> None: ... + def set_takeoff_origin( + self, + origin: LatLonAlt, + sigma_horiz_m: float, + sigma_vert_m: float, + ) -> None: ... def add_vio(self, vio: VioOutput) -> None: ... def add_pose_anchor(self, pose: PoseEstimate) -> None: ... @@ -46,7 +51,7 @@ class StateEstimator(Protocol): 8. **Spoof-rejection events ALWAYS land in FDR + GCS STATUSTEXT** — never silent (R07; C5-ST-01). 9. **AC-5.2 fallback on 3 s no-estimate** — if `current_estimate()` would raise OR the keyframe window is empty for ≥3 s, downstream C8 emits FC IMU-only. 10. **`covariance_6x6` is always SPD** — both strategies enforce; on numerical failure raise `EstimatorFatalError`. -11. **`set_takeoff_origin(origin, sigma_m)` is a `INIT`-state-only entrypoint** (AZ-490, ADR-010). Calling it after the estimator has transitioned to `TRACKING` raises `StateEstimatorConfigError`. Inside `INIT` it is idempotent — re-invocation overwrites the prior with the new origin + sigma. `sigma_m` MUST be positive and finite; otherwise raise `StateEstimatorConfigError`. The origin is consumed as a Bayesian prior on the initial pose key (iSAM2: `PriorFactorPose3` with covariance = `diag(sigma_m^2, sigma_m^2, (2*sigma_m)^2, ...)` in ENU position + orientation order; ESKF: nominal-state seed + position-block covariance = `sigma_m^2 * I_3`). +11. **`set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` is a cold-start-window-only entrypoint** (AZ-490, ADR-010). The cold-start window closes on the first `add_*` call; calling `set_takeoff_origin` after that raises `EstimatorAlreadyStartedError` (subclass of `StateEstimatorConfigError`). Inside the cold-start window the call is **strictly idempotent on identical args** (re-invocation with byte-equal `origin`/`sigma_horiz_m`/`sigma_vert_m` is a no-op); a re-invocation with **different** args raises `StateEstimatorConfigError`. Both sigmas MUST be positive and finite; the origin MUST be inside WGS-84 bounds (lat ∈ [-90, 90], lon ∈ [-180, 180], all components finite); otherwise raise `StateEstimatorConfigError`. The origin is consumed as a Bayesian prior on the first pose key (iSAM2: `PriorFactorPose3` at `Pose3.Identity()` with diagonal sigmas `[5°, 5°, 5°, sigma_horiz_m, sigma_horiz_m, sigma_vert_m]` in rotation+translation order; the operator origin BECOMES the local-ENU (0,0,0) anchor. ESKF: nominal-state position seeded to (0,0,0) + position block of error covariance set to `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`). One `c5.cold_start_origin.set` FDR record is emitted per estimator (with `source="manifest"` from this entrypoint, or `source="fc_ekf"` from the legacy first-frame path when no operator origin was supplied). 12. **Spoof-promotion gate has THREE clauses, not two** — Principle #11 amended. Re-promote a previously-spoofed FC GPS source only when ALL of: (i) FC `gps_health == STABLE_NON_SPOOFED` for ≥ `spoof_promotion_min_stable_s`, (ii) the next satellite-anchored frame agrees with the FC GPS within `spoof_promotion_visual_consistency_tol_m`, AND (iii) the FC's reported position is within `spoof_promotion_bounded_delta_m` (default 200 m) of the companion's last emitted `PoseEstimate`. The bounded-delta clause also gates the takeoff path when a Manifest `takeoff_origin` is present. ### DTOs (in `_types/state.py`) @@ -135,10 +140,11 @@ Config schema additions: | 8 | Spoof-rejection logging | FDR + GCS STATUSTEXT both fire on every gate decision | | 9 | AC-5.2 timeout | 3 s no estimate → fallback signal emitted | | 10 | SPD covariance | every emitted `covariance_6x6` is SPD | -| 11a | `set_takeoff_origin` after `TRACKING` | raises `StateEstimatorConfigError` | -| 11b | `set_takeoff_origin` with `sigma_m <= 0` or non-finite | raises `StateEstimatorConfigError` | -| 11c | `set_takeoff_origin` twice in `INIT` | second call wins; covariance updated to new sigma | -| 11d | First `current_estimate` after `set_takeoff_origin` + no sensor samples | returns `EstimatorOutput` with `position_wgs84 == origin`, `covariance_6x6` reflecting `sigma_m^2` in the position block | +| 11a | `set_takeoff_origin` after first `add_*` call | raises `EstimatorAlreadyStartedError` (subclass of `StateEstimatorConfigError`) | +| 11b | `set_takeoff_origin` with non-positive / non-finite sigma OR out-of-bounds `LatLonAlt` | raises `StateEstimatorConfigError` | +| 11c | `set_takeoff_origin` twice in cold-start window with **identical** args | second call is a no-op; no extra FDR record | +| 11d | `set_takeoff_origin` twice in cold-start window with **different** args | raises `StateEstimatorConfigError` (names both prev/new args) | +| 11e | First `current_estimate` after `set_takeoff_origin` + no sensor samples | returns `EstimatorOutput` with `position_wgs84 == origin`, position block of `covariance_6x6` reflecting `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)` | | 12 | Bounded-delta gate | FC GPS frame with |Δ| > 200 m vs last emitted `PoseEstimate` is rejected even when stable + non-spoofed for ≥ 10 s + visual-consistent | ## Producer-task / consumer-task split diff --git a/_docs/02_document/glossary.md b/_docs/02_document/glossary.md index 89f9927..c591f79 100644 --- a/_docs/02_document/glossary.md +++ b/_docs/02_document/glossary.md @@ -82,7 +82,7 @@ Terms are alphabetical. Each entry: one-line definition + parenthetical source. **Suite Sat Service** — Synonym for `satellite-provider` used in earlier docs (problem.md, restrictions.md, solution_draft01/02). The actual implementation in the parent suite is the .NET 8 service; "Suite Sat Service" is the role name. (source: `restrictions.md`, parent-suite `satellite-provider/README.md`) -**Takeoff origin** — `LatLonAlt` baked into the C10 Manifest by C12 at build time from `Flight.waypoints[0]`. Consumed at boot by C5 via `set_takeoff_origin(origin, sigma_m)` (AZ-490) as a Bayesian prior on the initial pose — iSAM2 attaches a `PriorFactorPose3`; ESKF seeds the nominal position + position-block covariance. Primary cold-start trust anchor per ADR-010; FC EKF GPS is secondary. (source: ADR-010, AZ-490) +**Takeoff origin** — `LatLonAlt` baked into the C10 Manifest by C12 at build time from `Flight.waypoints[0]`. Consumed at boot by C5 via `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` (AZ-490) as a Bayesian prior on the initial pose — iSAM2 attaches a `PriorFactorPose3` at `Pose3.Identity()` (origin BECOMES the local-ENU (0,0,0) anchor); ESKF seeds the nominal position to (0,0,0) + position-block covariance to `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`. Primary cold-start trust anchor per ADR-010; FC EKF GPS is secondary. (source: ADR-010, AZ-490) **Tier-1 / Tier-2** — Testing-environment split: Tier-1 = workstation Docker (fast/cheap); Tier-2 = Jetson hardware (AC-bound). Both appear in the deployment plan and CI matrix per finding F6. (source: `_docs/02_document/tests/environment.md`) diff --git a/_docs/02_document/system-flows.md b/_docs/02_document/system-flows.md index fff8b53..476fa3f 100644 --- a/_docs/02_document/system-flows.md +++ b/_docs/02_document/system-flows.md @@ -235,7 +235,7 @@ sequenceDiagram FC-->>Companion: first telemetry frame Note over Companion,Pipeline: Cold-start ladder (ADR-010, AZ-490). Operator-origin from Manifest is primary; FC EKF GPS is secondary alt Manifest carries takeoff_origin (AZ-490 primary path) - Companion->>Pipeline: C5.set_takeoff_origin(manifest.takeoff_origin, sigma_m) BEFORE any add_vio / add_fc_imu + Companion->>Pipeline: C5.set_takeoff_origin(manifest.takeoff_origin, sigma_horiz_m, sigma_vert_m) BEFORE any add_vio / add_fc_imu else Manifest has no takeoff_origin AND FC EKF GPS is valid (AZ-419 secondary path) Companion->>FC: query FC EKF last valid GPS + IMU-extrapolated pose (AC-5.1) FC-->>Companion: warm-start pose @@ -267,9 +267,9 @@ flowchart TD SignOk -->|no| RefuseTakeoff SignOk -->|yes| OriginGate InavOpen --> OriginGate{Manifest carries takeoff_origin?} - OriginGate -->|yes ADR-010 AZ-490 primary| OperatorOrigin[C5.set_takeoff_origin manifest.takeoff_origin sigma_m] + OriginGate -->|yes ADR-010 AZ-490 primary| OperatorOrigin[C5.set_takeoff_origin manifest.takeoff_origin sigma_horiz_m sigma_vert_m] OriginGate -->|no| FcEkfGate{FC EKF reports valid non-spoofed GPS?} - FcEkfGate -->|yes AZ-419 secondary| FcOrigin[C5.set_takeoff_origin fc_gps_origin fc_gps_sigma] + FcEkfGate -->|yes AZ-419 secondary| FcOrigin[C5.set_takeoff_origin fc_gps_origin fc_gps_sigma_horiz fc_gps_sigma_vert] FcEkfGate -->|no| NoOrigin[Stay INITIALIZING and apply FT-P-11 takeoff-abort policy] OperatorOrigin --> WarmPipeline FcOrigin --> WarmPipeline @@ -290,7 +290,7 @@ flowchart TD | 4 | Companion | C7 / TensorRT | `.engine` deserialize | TensorRT IRuntime | | 5 | Companion | FC (AP) | signing seed + handshake | MAVLink 2.0 signing | | 6 | FC | Companion | warm-start pose + IMU/attitude/GPS health | MAVLink (AP) / MSP2 + MAVLink outbound (iNav) | -| 7 | Companion | C5 `StateEstimator` (AZ-490) | `set_takeoff_origin(origin, sigma_m)` with origin = `manifest.takeoff_origin` (primary) OR FC-EKF GPS (secondary) | in-process Protocol method | +| 7 | Companion | C5 `StateEstimator` (AZ-490) | `set_takeoff_origin(origin, sigma_horiz_m, sigma_vert_m)` with origin = `manifest.takeoff_origin` (primary) OR FC-EKF GPS (secondary) | in-process Protocol method | | 8 | Companion | C13 FDR | startup record (config snapshot, signing key rotation event, content-hash digests, chosen cold-start origin source) | FDR record | ### Error scenarios diff --git a/_docs/02_tasks/todo/AZ-490_c5_set_takeoff_origin.md b/_docs/02_tasks/done/AZ-490_c5_set_takeoff_origin.md similarity index 100% rename from _docs/02_tasks/todo/AZ-490_c5_set_takeoff_origin.md rename to _docs/02_tasks/done/AZ-490_c5_set_takeoff_origin.md diff --git a/_docs/03_implementation/batch_22_cycle1_report.md b/_docs/03_implementation/batch_22_cycle1_report.md new file mode 100644 index 0000000..a37e2f1 --- /dev/null +++ b/_docs/03_implementation/batch_22_cycle1_report.md @@ -0,0 +1,210 @@ +# Batch 22 — AZ-490 C5 set_takeoff_origin + bounded-delta gate + +**Date**: 2026-05-12 +**Tracker**: Jira AZ-490 (Epic AZ-260 / E-C5) — transitioned To Do → In Progress → Done. +**Cycle**: 1 +**Status**: complete; 33 new unit tests green; full repo 750 passed / 2 skipped (pre-existing CI tooling skips). + +## Scope landed + +AZ-490 delivers ADR-010's airborne consumer of the operator-supplied takeoff +origin (the producer side landed in batch 21 as AZ-489). C5's `StateEstimator` +Protocol gains the pre-takeoff `set_takeoff_origin` entrypoint, both concrete +implementations (`GtsamIsam2StateEstimator` + `EskfStateEstimator`) honour it +with the same semantics, and the AZ-385 source-label state machine grows the +third — bounded-delta — clause of Principle #11. Four FDR record kinds +(`c5.cold_start_origin.set`, `c5.cold_start_origin.unavailable`, +`c5.gps_bounded_delta.accept`, `c5.gps_bounded_delta.reject`) are now part of +the AZ-272 schema and round-trip cleanly. + +The composition-root F2 wiring of `manifest.takeoff_origin → estimator.set_takeoff_origin` +remains explicitly out of scope (owned by the AZ-381 owner as a follow-up commit +to keep this task focused on the C5-side contract per the spec). + +### Public surface + +* **`StateEstimator` Protocol** (`src/gps_denied_onboard/components/c5_state/interface.py`) + gains `set_takeoff_origin(origin: LatLonAlt, sigma_horiz_m: float, sigma_vert_m: float) -> None`. + Two-sigma signature replaces the contract's older single-`sigma_m` shape per + the AZ-490 task spec — see "Decisions" below. +* **`GtsamIsam2StateEstimator`** seeds the iSAM2 graph with one + `PriorFactorPose3` at `Pose3.Identity()` (operator origin BECOMES the local + ENU (0,0,0) anchor) with diagonal sigmas + `[5°, 5°, 5°, sigma_horiz_m, sigma_horiz_m, sigma_vert_m]` and drives a + single `handle.update`. +* **`EskfStateEstimator`** zeros `_nominal_pos` and overwrites the position + block of `_P` to `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`, + defensively re-symmetrising afterwards. +* **`SourceLabelStateMachine.process_gps_sample(sample, smoother_estimate, now_ns)`** + is the third clause of the spoof-promotion gate. Returns + `BOUNDED_DELTA_SOFT` when the sample is within + `spoof_promotion_bounded_delta_m` of the smoother estimate (one + `c5.gps_bounded_delta.accept` FDR record); returns `BOUNDED_DELTA_REJECT` + when outside the ring (one `c5.gps_bounded_delta.reject` FDR record + the + dwell-time clause counter is reset, so the rejection counts against the + existing AZ-385 promotion path); returns `None` when no smoother estimate + is available yet (cold start). Distance is computed via + `WgsConverter.horizontal_distance_m`, matching the WGS-84 geodesic + (pyproj's ECEF chain) within sub-mm at the bounded-delta operating range. +* **Both estimators** expose `notify_gps_sample(sample: GpsSample) -> str | None` + as the C8-inbound seam — looks up the smoother's current latlon + (best-effort) and delegates to the source-label machine. +* **Errors** — `EstimatorAlreadyStartedError` is a new + `StateEstimatorConfigError` subclass for the late-call path. Existing + `except StateEstimatorConfigError` callers still catch it. +* **Config** — `C5StateConfig` gains `spoof_promotion_bounded_delta_m=200.0`, + `default_takeoff_origin_sigma_horiz_m=5.0`, `default_takeoff_origin_sigma_vert_m=10.0` + with `__post_init__` positivity validation matching the existing pattern. +* **DTO** — new `GpsSample` in `src/gps_denied_onboard/_types/fc.py` carries + `LatLonAlt` + `captured_at: int`. Distinct from `GpsHealth` (which only + carries the health enum); the bounded-delta gate needs the position. +* **WgsConverter** — new static `WgsConverter.horizontal_distance_m(a, b)` + reuses the existing `latlonalt_to_local_enu` ECEF transform and returns + `hypot(east, north)`. Geodetically correct (NOT haversine on + equirectangular per AC-15). +* **FDR schema** — four new kinds registered in + `KNOWN_PAYLOAD_KEYS` with explicit per-key allow-lists; AZ-268 contract + test (`test_az272_fdr_record_schema.py`) was extended with their fixture + payloads and now round-trips them. + +### Cold-start ledger + +* The cold-start window closes on the first `add_*` call (vio / pose_anchor / + fc_imu) via the new `_close_cold_start_window` helper added to both + estimators. `set_takeoff_origin` after the window closes raises + `EstimatorAlreadyStartedError`. +* Exactly one `c5.cold_start_origin.set` FDR record is emitted per + estimator: `source="manifest"` from `set_takeoff_origin`, or + `source="fc_ekf"` from `_close_cold_start_window` if no operator origin + was supplied (legacy AZ-419 fallback path, just newly logged per AC-13). +* Idempotency: re-calling `set_takeoff_origin` with byte-identical args is a + no-op (no second FDR, no second prior). Re-calling with different args + raises `StateEstimatorConfigError` naming both prev/new args. + +### Doc updates + +* `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — + signature widened to two sigmas; Invariants 11a/11b/11c/11d/11e refined + to match the implemented contract; Invariant 12 (bounded-delta gate) + unchanged. +* `_docs/02_document/components/07_c5_state/description.md` — params + column, state-management cold-start ladder, and error-handling table + updated. New `EstimatorAlreadyStartedError` documented. +* `_docs/02_document/architecture.md` — ADR-010 narrative + `TakeoffOrigin` + glossary line + Principle #14 narrative updated. +* `_docs/02_document/system-flows.md` — F2 sequence diagram + flow chart + + data-table all carry the two-sigma signature. +* `_docs/02_document/glossary.md` — `Takeoff origin` entry updated. +* `_docs/02_document/components/11_c10_provisioning/description.md` — + one prose mention updated. + +## Tests + +* `tests/unit/c5_state/test_az490_set_takeoff_origin.py` — 33 tests + covering AC-1..AC-15 plus three additional edge cases (no-smoother + bounded-delta returns None; ESKF `notify_gps_sample` happy path; protocol + conformance for both impls). All pass. +* `tests/unit/c5_state/test_az381_state_protocol.py` — `_FakeEstimator` + test fake updated with the new `set_takeoff_origin` method (isinstance + check stays green). +* `tests/unit/c5_state/test_az385_source_label_spoof_gate.py` — fixture + `_make_sm` now passes `spoof_promotion_bounded_delta_m=200.0` to the SM + constructor; existing AC-1..AC-12 still pass (AC-14 of AZ-490 — additive + clause). +* `tests/unit/test_az272_fdr_record_schema.py` — `_kind_payload` extended + with the four new C5 kinds; the `KNOWN_KINDS`-parametrised + round-trip test now covers them automatically. + +Full repo: **750 passed / 2 skipped** (the two skips are pre-existing +CI-only tooling skips: `cmake` and `actionlint` not on PATH locally). + +## Decisions + +* **Two-sigma signature, not single-sigma**: the C5 contract's older + signature (`sigma_m: float`) lost a useful axis — operator-supplied + manifests typically distinguish horizontal (GPS) from vertical + (barometric / GPS) uncertainty by an order of magnitude. The task spec's + two-sigma form is more general and aligns with the new + `default_takeoff_origin_sigma_{horiz,vert}_m` config defaults. The + contract document was updated to match. +* **`EstimatorAlreadyStartedError` as a `StateEstimatorConfigError` + subclass**, not a peer: existing `except StateEstimatorConfigError` + callers should still catch it; a new exception name is justified by the + semantic distinction (you called the method at the wrong time, vs. you + called it with bad args), but the inheritance keeps the C5 error + hierarchy three-rooted (Degraded / Fatal / Config). +* **`BOUNDED_DELTA_SOFT` is a private string constant**, not a new + `PoseSourceLabel` enum value: `PoseSourceLabel` is a shared C4/C5 public + surface; adding a value would ripple through every consumer (test + fixtures, FDR `state.tick` schema, C8 outbound formatter) for a soft- + admission outcome that doesn't change the externally observable pose + provenance. The string constant is exposed from `_source_label_sm` for + the ESKF / iSAM2 dispatch return value but never enters the public + `EstimatorOutput.source_label` field. +* **`WgsConverter.horizontal_distance_m` instead of a new + `vincenty_distance` method**: AC-15's text references "Vincenty + distance" but pyproj's ECEF chain (which our `latlonalt_to_local_enu` + already uses) matches Vincenty within sub-mm at the bounded-delta + operating range (<= ~1 km), so the algorithmic family is geodetically + correct. The new helper makes the intent explicit. AC-15 is satisfied + against `pyproj.Geod.inv` as the WGS-84 reference (the test compares to + the geodesic, not a haversine shortcut). +* **`GpsSample` DTO introduced now**, even though full inbound wiring from + AZ-391 is deferred. The bounded-delta method needs a typed handle on + `(LatLonAlt, captured_at)` — extending `GpsHealth` to carry a position + was rejected as it conflates "health enum" with "geographic sample". + The `notify_gps_sample(sample)` method on both estimators is the + composition-root seam; AZ-391 plumbing arrives in a later task. +* **Bounded-delta reject also resets the dwell-time clause**: a wildly-off + GPS sample is a strong signal the FC stream is unreliable, even when + `gps_health == STABLE_NON_SPOOFED`. Resetting `_gps_health_stable_since_ns` + on reject means the AZ-385 spoof gate can't re-promote off the back of + bad data. AC-10 verifies this. + +## Self-review findings + +* **Low / Code-quality**: `_validate_takeoff_origin_args` is duplicated + between `gtsam_isam2_estimator.py` (static method) and `eskf_baseline.py` + (module-level helper). Same source code. Can be lifted to a shared + `_validation.py` if the duplication grows. Surgical for now — + intentionally not refactored to keep the AZ-490 diff scoped. + +No High / Critical findings. PASS_WITH_WARNINGS verdict. + +## Out-of-scope (explicit) + +* Composition-root F2 wiring of + `manifest.takeoff_origin → estimator.set_takeoff_origin` (owned by the + AZ-381 owner as a follow-up commit per the task spec). +* AZ-391 inbound-path wiring of `GpsSample` (FC GPS observations currently + flow as `GpsHealth` only; `GpsSample` is a forward-compatible DTO that a + later task will populate from the C8 inbound subscription). +* C10 Manifest schema extension to include `takeoff_origin` (owned by + AZ-323 / AZ-324; the consumer side is in place). +* AZ-419's BBT cold-start path validation (FT-P-11 — owned separately). + +## Files touched + +``` +src/gps_denied_onboard/_types/fc.py +src/gps_denied_onboard/components/c5_state/__init__.py +src/gps_denied_onboard/components/c5_state/_source_label_sm.py +src/gps_denied_onboard/components/c5_state/config.py +src/gps_denied_onboard/components/c5_state/errors.py +src/gps_denied_onboard/components/c5_state/eskf_baseline.py +src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py +src/gps_denied_onboard/components/c5_state/interface.py +src/gps_denied_onboard/fdr_client/records.py +src/gps_denied_onboard/helpers/wgs_converter.py +tests/unit/c5_state/test_az381_state_protocol.py +tests/unit/c5_state/test_az385_source_label_spoof_gate.py +tests/unit/c5_state/test_az490_set_takeoff_origin.py [new] +tests/unit/test_az272_fdr_record_schema.py +_docs/02_document/architecture.md +_docs/02_document/components/07_c5_state/description.md +_docs/02_document/components/11_c10_provisioning/description.md +_docs/02_document/contracts/c5_state/state_estimator_protocol.md +_docs/02_document/glossary.md +_docs/02_document/system-flows.md +_docs/03_implementation/batch_22_cycle1_report.md [new] +``` diff --git a/_docs/_autodev_state.md b/_docs/_autodev_state.md index edf9f40..307d3c3 100644 --- a/_docs/_autodev_state.md +++ b/_docs/_autodev_state.md @@ -8,7 +8,7 @@ status: in_progress sub_step: phase: 6 name: implement-tasks - detail: "batch 22 of N landed (AZ-489 — C12 FlightsApiClient + offline JSON loader + bbox helper + httpx client). httpx>=0.28,<1.0 added to main deps. 28 unit tests covering AC-1..AC-18 plus extras; full repo 713 passed / 2 skipped. Jira AZ-489 transitioned To Do -> In Progress -> Done; spec file moved to _docs/02_tasks/done/. OperatorToolServices aggregate intentionally deferred to AZ-328 per scope discipline. Next: AZ-490 (C5 set_takeoff_origin entrypoint + bounded-delta gate)." + detail: "" retry_count: 0 cycle: 1 tracker: jira diff --git a/src/gps_denied_onboard/_types/fc.py b/src/gps_denied_onboard/_types/fc.py index 0f9b8d3..f959394 100644 --- a/src/gps_denied_onboard/_types/fc.py +++ b/src/gps_denied_onboard/_types/fc.py @@ -28,6 +28,7 @@ __all__ = [ "FlightState", "FlightStateSignal", "GpsHealth", + "GpsSample", "GpsStatus", "ImuTelemetrySample", "OperatorCommand", @@ -142,6 +143,22 @@ class GpsHealth: captured_at: int +@dataclass(frozen=True, slots=True) +class GpsSample: + """Single FC-reported GPS lat/lon/alt sample (post-decode form). + + Distinct from :class:`GpsHealth` (which carries only the health + status enum) — this is the geographic position the FC reports + alongside the health bucket. Consumed by C5's bounded-delta gate + (AZ-490 / Principle #11 third clause) as the `vincenty(sample, + smoother)` input. ``captured_at`` is monotonic_ns at the decode + boundary, mirroring :class:`GpsHealth` for Invariant 7. + """ + + position_wgs84: LatLonAlt + captured_at: int + + @dataclass(frozen=True, slots=True) class FlightStateSignal: """FC's high-level flight-state lattice + AC-5.1 warm-start hint.""" diff --git a/src/gps_denied_onboard/components/c5_state/__init__.py b/src/gps_denied_onboard/components/c5_state/__init__.py index 2279db9..b73e38b 100644 --- a/src/gps_denied_onboard/components/c5_state/__init__.py +++ b/src/gps_denied_onboard/components/c5_state/__init__.py @@ -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", diff --git a/src/gps_denied_onboard/components/c5_state/_source_label_sm.py b/src/gps_denied_onboard/components/c5_state/_source_label_sm.py index 3d3ee19..fd0f5c2 100644 --- a/src/gps_denied_onboard/components/c5_state/_source_label_sm.py +++ b/src/gps_denied_onboard/components/c5_state/_source_label_sm.py @@ -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. diff --git a/src/gps_denied_onboard/components/c5_state/config.py b/src/gps_denied_onboard/components/c5_state/config.py index 4d2e84d..6b5ae02 100644 --- a/src/gps_denied_onboard/components/c5_state/config.py +++ b/src/gps_denied_onboard/components/c5_state/config.py @@ -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; " diff --git a/src/gps_denied_onboard/components/c5_state/errors.py b/src/gps_denied_onboard/components/c5_state/errors.py index 5f2842b..1abedda 100644 --- a/src/gps_denied_onboard/components/c5_state/errors.py +++ b/src/gps_denied_onboard/components/c5_state/errors.py @@ -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. """ diff --git a/src/gps_denied_onboard/components/c5_state/eskf_baseline.py b/src/gps_denied_onboard/components/c5_state/eskf_baseline.py index 458672d..8f013f1 100644 --- a/src/gps_denied_onboard/components/c5_state/eskf_baseline.py +++ b/src/gps_denied_onboard/components/c5_state/eskf_baseline.py @@ -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``. diff --git a/src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py b/src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py index 52d7f84..41ff6ac 100644 --- a/src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py +++ b/src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py @@ -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: diff --git a/src/gps_denied_onboard/components/c5_state/interface.py b/src/gps_denied_onboard/components/c5_state/interface.py index 6b7b20e..9a00d22 100644 --- a/src/gps_denied_onboard/components/c5_state/interface.py +++ b/src/gps_denied_onboard/components/c5_state/interface.py @@ -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.""" diff --git a/src/gps_denied_onboard/fdr_client/records.py b/src/gps_denied_onboard/fdr_client/records.py index 3a4c10f..1cb96f7 100644 --- a/src/gps_denied_onboard/fdr_client/records.py +++ b/src/gps_denied_onboard/fdr_client/records.py @@ -69,6 +69,31 @@ KNOWN_PAYLOAD_KEYS: Final[dict[str, frozenset[str]]] = { "clean_shutdown", } ), + # AZ-490 / E-C5: operator-origin cold-start ladder + bounded-delta GPS gate. + "c5.cold_start_origin.set": frozenset( + {"source", "lat_deg", "lon_deg", "alt_m", "sigma_horiz_m", "sigma_vert_m"} + ), + "c5.cold_start_origin.unavailable": frozenset({"reason"}), + "c5.gps_bounded_delta.accept": frozenset( + { + "sample_lat", + "sample_lon", + "smoother_lat", + "smoother_lon", + "distance_m", + "threshold_m", + } + ), + "c5.gps_bounded_delta.reject": frozenset( + { + "sample_lat", + "sample_lon", + "smoother_lat", + "smoother_lon", + "distance_m", + "threshold_m", + } + ), } KNOWN_KINDS: Final[frozenset[str]] = frozenset(KNOWN_PAYLOAD_KEYS.keys()) diff --git a/src/gps_denied_onboard/helpers/wgs_converter.py b/src/gps_denied_onboard/helpers/wgs_converter.py index 840743c..cbc93f6 100644 --- a/src/gps_denied_onboard/helpers/wgs_converter.py +++ b/src/gps_denied_onboard/helpers/wgs_converter.py @@ -91,6 +91,29 @@ class WgsConverter: delta_ecef = rotation @ p_enu.astype(np.float64) return WgsConverter.ecef_to_latlonalt(origin_ecef + delta_ecef) + @staticmethod + def horizontal_distance_m(a: LatLonAlt, b: LatLonAlt) -> float: + """Return the geodesic horizontal distance (m) between ``a`` and ``b``. + + Backed by the same ``pyproj`` ECEF transformer that powers + :meth:`latlonalt_to_local_enu`: convert ``b`` into the + local-ENU frame anchored at ``a`` and take ``hypot(east, + north)``. The altitude component is ignored — this is the + flat-distance over the WGS-84 ellipsoid, NOT a 3-D distance. + + Accuracy: pyproj's ECEF chain matches Vincenty within sub-mm + at horizontal separations ≤ a few km (the bounded-delta gate + operates at ≤ ~1 km), so AZ-490's "Vincenty distance" AC is + satisfied — the algorithmic family is geodetically correct, + not the haversine-on-equirectangular shortcut the AC excludes. + """ + _validate_finite_latlonalt(a, "horizontal_distance_m/a") + _validate_finite_latlonalt(b, "horizontal_distance_m/b") + enu = WgsConverter.latlonalt_to_local_enu(a, b) + east = float(enu[0]) + north = float(enu[1]) + return math.hypot(east, north) + @staticmethod def latlon_to_tile_xy(zoom: int, lat: float, lon: float) -> tuple[int, int]: _validate_zoom(zoom) diff --git a/tests/unit/c5_state/test_az381_state_protocol.py b/tests/unit/c5_state/test_az381_state_protocol.py index 23dd9b6..eeaf6d7 100644 --- a/tests/unit/c5_state/test_az381_state_protocol.py +++ b/tests/unit/c5_state/test_az381_state_protocol.py @@ -93,6 +93,14 @@ def _build_config(**state_overrides: Any) -> Config: class _FakeEstimator: """Test fake satisfying every StateEstimator method (AC-1).""" + def set_takeoff_origin( + self, + origin: LatLonAlt, + sigma_horiz_m: float, + sigma_vert_m: float, + ) -> None: + pass + def add_vio(self, vio: VioOutput) -> None: pass @@ -133,7 +141,7 @@ def test_ac1_missing_method_fails_isinstance() -> None: def add_vio(self, vio: VioOutput) -> None: pass - # Assert — missing 5 methods → not a StateEstimator + # Assert — missing 6 methods → not a StateEstimator assert not isinstance(_Incomplete(), StateEstimator) diff --git a/tests/unit/c5_state/test_az385_source_label_spoof_gate.py b/tests/unit/c5_state/test_az385_source_label_spoof_gate.py index 0167e15..ab94858 100644 --- a/tests/unit/c5_state/test_az385_source_label_spoof_gate.py +++ b/tests/unit/c5_state/test_az385_source_label_spoof_gate.py @@ -71,6 +71,7 @@ def _make_sm( *, min_stable_s: float = 10.0, tol_m: float = 30.0, + bounded_delta_m: float = 200.0, clock: _Clock | None = None, fdr_client: mock.MagicMock | None = None, ) -> tuple[SourceLabelStateMachine, _Clock, mock.MagicMock]: @@ -80,6 +81,7 @@ def _make_sm( sm = SourceLabelStateMachine( spoof_promotion_min_stable_s=min_stable_s, spoof_promotion_visual_consistency_tol_m=tol_m, + spoof_promotion_bounded_delta_m=bounded_delta_m, fdr_client=fdr, producer_id="c5_state", clock_ns=clock, diff --git a/tests/unit/c5_state/test_az490_set_takeoff_origin.py b/tests/unit/c5_state/test_az490_set_takeoff_origin.py new file mode 100644 index 0000000..8f3bc0f --- /dev/null +++ b/tests/unit/c5_state/test_az490_set_takeoff_origin.py @@ -0,0 +1,647 @@ +"""AZ-490 — ``set_takeoff_origin`` + bounded-delta GPS gate. + +Covers AC-1..AC-15 from +``_docs/02_tasks/todo/AZ-490_c5_set_takeoff_origin.md``. + +The tests construct estimators directly (the iSAM2 estimator +exercises the real GTSAM ``PriorFactorPose3`` insertion; the ESKF +estimator exercises the real NumPy covariance write). The +``SourceLabelStateMachine`` is exercised in isolation for the +bounded-delta clauses (AC-9..AC-11, AC-15) to keep the iSAM2 +machinery out of the gate-only tests. +""" + +from __future__ import annotations + +import dataclasses +import math +from datetime import datetime, timezone +from typing import Any +from unittest import mock +from uuid import UUID + +import numpy as np +import pytest + +from gps_denied_onboard._types.fc import GpsSample +from gps_denied_onboard._types.geo import LatLonAlt +from gps_denied_onboard._types.pose import ( + CovarianceMode, + PoseEstimate, + Quat, +) +from gps_denied_onboard._types.state import PoseSourceLabel +from gps_denied_onboard._types.vio import VioOutput +from gps_denied_onboard.components.c5_state import ( + C5StateConfig, + EstimatorAlreadyStartedError, + StateEstimator, + StateEstimatorConfigError, +) +from gps_denied_onboard.components.c5_state._source_label_sm import ( + BOUNDED_DELTA_REJECT, + BOUNDED_DELTA_SOFT, + SourceLabelStateMachine, +) +from gps_denied_onboard.components.c5_state.eskf_baseline import EskfStateEstimator +from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import ( + GtsamIsam2StateEstimator, + create as create_isam2, +) +from gps_denied_onboard.fdr_client.records import FdrRecord, parse, serialise +from gps_denied_onboard.helpers.wgs_converter import WgsConverter +from gps_denied_onboard.runtime_root.state_factory import clear_state_registry + + +# --------------------------------------------------------------------- +# Fixtures + builders. + + +@pytest.fixture(autouse=True) +def _registry_isolation() -> Any: + clear_state_registry() + yield + clear_state_registry() + + +def _build_isam2(**overrides: Any) -> GtsamIsam2StateEstimator: + block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15, **overrides) + cfg = mock.MagicMock() + cfg.components = {"c5_state": block} + estimator, _ = create_isam2( + config=cfg, + imu_preintegrator=mock.MagicMock(), + se3_utils=mock.MagicMock(), + wgs_converter=mock.MagicMock(), + fdr_client=mock.MagicMock(), + ) + return estimator + + +def _build_eskf(**overrides: Any) -> EskfStateEstimator: + block = C5StateConfig(strategy="eskf", keyframe_window_size=15, **overrides) + cfg = mock.MagicMock() + cfg.components = {"c5_state": block} + return EskfStateEstimator( + cfg, + imu_preintegrator=mock.MagicMock(), + se3_utils=mock.MagicMock(), + wgs_converter=mock.MagicMock(), + fdr_client=mock.MagicMock(), + ) + + +def _origin() -> LatLonAlt: + return LatLonAlt(lat_deg=50.0, lon_deg=36.2, alt_m=200.0) + + +def _vio(*, frame_id: int, t_seconds: float) -> VioOutput: + return VioOutput( + frame_id=frame_id, + timestamp=datetime.fromtimestamp(t_seconds, tz=timezone.utc), + pose_se3=np.eye(4), + covariance_6x6=np.eye(6) * 0.01, + ) + + +def _pose_anchor(*, frame_id: int, t_seconds: float) -> PoseEstimate: + return PoseEstimate( + frame_id=UUID(int=frame_id), + position_wgs84=_origin(), + orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0), + covariance_6x6=np.eye(6) * 0.01, + covariance_mode=CovarianceMode.MARGINALS, + source_label=PoseSourceLabel.SATELLITE_ANCHORED, + last_satellite_anchor_age_ms=0, + emitted_at=int(t_seconds * 1_000_000_000), + ) + + +def _cold_start_records(estimator: GtsamIsam2StateEstimator | EskfStateEstimator) -> list[FdrRecord]: + enqueued = estimator._fdr_client.enqueue.call_args_list # type: ignore[union-attr] + out = [] + for call in enqueued: + args, _ = call + if not args: + continue + record = args[0] + if isinstance(record, FdrRecord) and record.kind == "c5.cold_start_origin.set": + out.append(record) + return out + + +def _bounded_delta_records( + fdr: mock.MagicMock, *, kind: str +) -> list[FdrRecord]: + out = [] + for call in fdr.enqueue.call_args_list: + args, _ = call + if args and isinstance(args[0], FdrRecord) and args[0].kind == kind: + out.append(args[0]) + return out + + +class _Clock: + """Mutable monotonic clock used by SourceLabelStateMachine in tests.""" + + def __init__(self, t0: int = 0) -> None: + self.t = t0 + + def __call__(self) -> int: + return self.t + + +def _make_sm( + *, + bounded_delta_m: float = 200.0, + fdr_client: mock.MagicMock | None = None, + clock: _Clock | None = None, +) -> tuple[SourceLabelStateMachine, mock.MagicMock]: + fdr = fdr_client if fdr_client is not None else mock.MagicMock() + sm = SourceLabelStateMachine( + spoof_promotion_min_stable_s=10.0, + spoof_promotion_visual_consistency_tol_m=30.0, + spoof_promotion_bounded_delta_m=bounded_delta_m, + fdr_client=fdr, + producer_id="c5_state", + clock_ns=clock if clock is not None else _Clock(0), + ) + return sm, fdr + + +# --------------------------------------------------------------------- +# AC-1: Protocol conformance — both impls expose set_takeoff_origin. + + +def test_ac1_isam2_is_state_estimator() -> None: + estimator = _build_isam2() + assert isinstance(estimator, StateEstimator) + assert callable(estimator.set_takeoff_origin) + + +def test_ac1_eskf_is_state_estimator() -> None: + estimator = _build_eskf() + assert isinstance(estimator, StateEstimator) + assert callable(estimator.set_takeoff_origin) + + +# --------------------------------------------------------------------- +# AC-2: iSAM2 happy path — origin seeds the smoother prior. + + +def test_ac2_isam2_set_takeoff_origin_seeds_prior_and_emits_fdr() -> None: + # Arrange + estimator = _build_isam2() + + # Act + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + + # Assert — exactly one PriorFactorPose3 was committed + import gtsam + + factors = estimator._isam2.getFactorsUnsafe() + prior_factors = [ + factors.at(i) + for i in range(factors.size()) + if isinstance(factors.at(i), gtsam.PriorFactorPose3) + ] + assert len(prior_factors) == 1 + # Pose is at Identity (origin == ENU(0,0,0)) + pose = prior_factors[0].prior() + assert np.allclose(np.asarray(pose.translation()), np.zeros(3), atol=1e-9) + + # Assert — exactly one cold-start FDR record with source="manifest" + records = _cold_start_records(estimator) + assert len(records) == 1 + assert records[0].payload["source"] == "manifest" + assert records[0].payload["lat_deg"] == _origin().lat_deg + assert records[0].payload["sigma_horiz_m"] == 5.0 + assert records[0].payload["sigma_vert_m"] == 10.0 + + # Assert — ENU origin tracks the operator origin + assert estimator._enu_origin == _origin() + + +# --------------------------------------------------------------------- +# AC-3: ESKF happy path — nominal state + P block seeded. + + +def test_ac3_eskf_set_takeoff_origin_seeds_state_and_p() -> None: + # Arrange + estimator = _build_eskf() + + # Act + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + + # Assert — nominal position is zero (origin IS the ENU(0,0,0) anchor) + assert np.allclose(estimator._nominal_pos, np.zeros(3), atol=1e-12) + # Assert — position block of P matches diag(25, 25, 100) + expected = np.diag([25.0, 25.0, 100.0]) + assert np.allclose(estimator._P[0:3, 0:3], expected, atol=1e-12) + # Assert — ENU origin tracks the operator origin + assert estimator._enu_origin == _origin() + # Assert — exactly one FDR record + records = _cold_start_records(estimator) + assert len(records) == 1 + assert records[0].payload["source"] == "manifest" + + +# --------------------------------------------------------------------- +# AC-4: Idempotent — calling twice with identical args is a no-op. + + +@pytest.mark.parametrize("builder", [_build_isam2, _build_eskf]) +def test_ac4_idempotent_double_call_is_noop(builder: Any) -> None: + # Arrange + estimator = builder() + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + fdr_calls_before = len(estimator._fdr_client.enqueue.call_args_list) + + # Act — second identical call + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + + # Assert — no extra FDR record, no exception + fdr_calls_after = len(estimator._fdr_client.enqueue.call_args_list) + assert fdr_calls_after == fdr_calls_before + + +# --------------------------------------------------------------------- +# AC-5: Conflict — calling twice with different args raises. + + +@pytest.mark.parametrize("builder", [_build_isam2, _build_eskf]) +def test_ac5_conflict_double_call_raises(builder: Any) -> None: + # Arrange + estimator = builder() + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + + # Act + Assert — different origin + different = LatLonAlt(lat_deg=51.0, lon_deg=36.2, alt_m=200.0) + with pytest.raises(StateEstimatorConfigError) as excinfo: + estimator.set_takeoff_origin(different, sigma_horiz_m=5.0, sigma_vert_m=10.0) + msg = str(excinfo.value) + # Both old and new origins named in the message + assert "previous=" in msg and "new=" in msg + + +# --------------------------------------------------------------------- +# AC-6: Late call — set_takeoff_origin after first add_vio raises. + + +def test_ac6_isam2_late_call_raises_already_started() -> None: + # Arrange + estimator = _build_isam2() + estimator._isam2_handle = mock.MagicMock() # stub the handle for add_vio + + # Act — first add_vio closes the cold-start window + estimator.add_vio(_vio(frame_id=1, t_seconds=1.0)) + + # Assert + with pytest.raises(EstimatorAlreadyStartedError): + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + + +def test_ac6_eskf_late_call_raises_already_started() -> None: + # Arrange + estimator = _build_eskf() + estimator.add_vio(_vio(frame_id=1, t_seconds=1.0)) + + # Assert + with pytest.raises(EstimatorAlreadyStartedError): + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + + +# --------------------------------------------------------------------- +# AC-7: Out-of-bounds latitude raises StateEstimatorConfigError. + + +@pytest.mark.parametrize("builder", [_build_isam2, _build_eskf]) +def test_ac7_invalid_latlonalt_raises(builder: Any) -> None: + # Arrange + estimator = builder() + bad_origin = LatLonAlt(lat_deg=95.0, lon_deg=36.0, alt_m=200.0) + + # Act + Assert + with pytest.raises(StateEstimatorConfigError, match=r"latitude 95\.0 outside"): + estimator.set_takeoff_origin(bad_origin, sigma_horiz_m=5.0, sigma_vert_m=10.0) + + +@pytest.mark.parametrize("builder", [_build_isam2, _build_eskf]) +def test_ac7_invalid_longitude_raises(builder: Any) -> None: + estimator = builder() + bad = LatLonAlt(lat_deg=50.0, lon_deg=200.0, alt_m=200.0) + with pytest.raises(StateEstimatorConfigError, match=r"longitude 200\.0 outside"): + estimator.set_takeoff_origin(bad, sigma_horiz_m=5.0, sigma_vert_m=10.0) + + +@pytest.mark.parametrize("builder", [_build_isam2, _build_eskf]) +def test_ac7_non_finite_lat_raises(builder: Any) -> None: + estimator = builder() + bad = LatLonAlt(lat_deg=float("nan"), lon_deg=36.0, alt_m=200.0) + with pytest.raises(StateEstimatorConfigError, match="non-finite"): + estimator.set_takeoff_origin(bad, sigma_horiz_m=5.0, sigma_vert_m=10.0) + + +# --------------------------------------------------------------------- +# AC-8: Negative sigma raises. + + +@pytest.mark.parametrize("builder", [_build_isam2, _build_eskf]) +def test_ac8_negative_horiz_sigma_raises(builder: Any) -> None: + estimator = builder() + with pytest.raises(StateEstimatorConfigError, match="sigma_horiz_m must be positive"): + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=-5.0, sigma_vert_m=10.0) + + +@pytest.mark.parametrize("builder", [_build_isam2, _build_eskf]) +def test_ac8_zero_vert_sigma_raises(builder: Any) -> None: + estimator = builder() + with pytest.raises(StateEstimatorConfigError, match="sigma_vert_m must be positive"): + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=0.0) + + +# --------------------------------------------------------------------- +# AC-9: Bounded-delta accept — sample within ring is admitted. + + +def test_ac9_bounded_delta_accept_emits_soft_label() -> None: + # Arrange + sm, fdr = _make_sm(bounded_delta_m=200.0) + smoother_estimate = LatLonAlt(lat_deg=50.0, lon_deg=36.2, alt_m=200.0) + sample = LatLonAlt(lat_deg=50.0008, lon_deg=36.2008, alt_m=200.0) + + # Sanity — distance is well under the ring (≈ 100 m at 50° N). + distance = WgsConverter.horizontal_distance_m(smoother_estimate, sample) + assert distance < 200.0 + + # Act + result = sm.process_gps_sample(sample, smoother_estimate=smoother_estimate) + + # Assert + assert result == BOUNDED_DELTA_SOFT + accepts = _bounded_delta_records(fdr, kind="c5.gps_bounded_delta.accept") + assert len(accepts) == 1 + assert accepts[0].payload["distance_m"] == pytest.approx(distance, rel=1e-9) + assert accepts[0].payload["threshold_m"] == 200.0 + + +# --------------------------------------------------------------------- +# AC-10: Bounded-delta reject — sample outside ring is dropped. + + +def test_ac10_bounded_delta_reject_emits_record_and_resets_dwell() -> None: + # Arrange + sm, fdr = _make_sm(bounded_delta_m=200.0) + smoother_estimate = LatLonAlt(lat_deg=50.0, lon_deg=36.2, alt_m=200.0) + sample = LatLonAlt(lat_deg=50.005, lon_deg=36.205, alt_m=200.0) + distance = WgsConverter.horizontal_distance_m(smoother_estimate, sample) + assert distance > 200.0 + + # Seed a STABLE_NON_SPOOFED dwell so we can observe its reset + from gps_denied_onboard._types.fc import GpsHealth, GpsStatus + + sm.notify_gps_health( + GpsHealth(status=GpsStatus.STABLE_NON_SPOOFED, fix_age_ms=10, captured_at=0) + ) + assert sm._gps_health_stable_since_ns is not None # arrange precondition + + # Act + result = sm.process_gps_sample(sample, smoother_estimate=smoother_estimate) + + # Assert + assert result == BOUNDED_DELTA_REJECT + rejects = _bounded_delta_records(fdr, kind="c5.gps_bounded_delta.reject") + assert len(rejects) == 1 + rec = rejects[0].payload + assert rec["sample_lat"] == sample.lat_deg + assert rec["smoother_lat"] == smoother_estimate.lat_deg + assert rec["distance_m"] == pytest.approx(distance, rel=1e-9) + # Dwell-time clause was reset by the reject + assert sm._gps_health_stable_since_ns is None + + +# --------------------------------------------------------------------- +# AC-11: Threshold is config-driven — relaxing it admits AC-10's sample. + + +def test_ac11_threshold_relaxed_admits_previously_rejected_sample() -> None: + sm, _fdr = _make_sm(bounded_delta_m=1000.0) + smoother_estimate = LatLonAlt(lat_deg=50.0, lon_deg=36.2, alt_m=200.0) + sample = LatLonAlt(lat_deg=50.005, lon_deg=36.205, alt_m=200.0) + + result = sm.process_gps_sample(sample, smoother_estimate=smoother_estimate) + assert result == BOUNDED_DELTA_SOFT + + +# --------------------------------------------------------------------- +# AC-12: FDR record kinds round-trip through the schema. + + +@pytest.mark.parametrize( + ("kind", "payload"), + [ + ( + "c5.cold_start_origin.set", + { + "source": "manifest", + "lat_deg": 50.0, + "lon_deg": 36.2, + "alt_m": 200.0, + "sigma_horiz_m": 5.0, + "sigma_vert_m": 10.0, + }, + ), + ( + "c5.cold_start_origin.unavailable", + {"reason": "no_manifest_origin_no_fc_ekf"}, + ), + ( + "c5.gps_bounded_delta.accept", + { + "sample_lat": 50.0, + "sample_lon": 36.2, + "smoother_lat": 50.0001, + "smoother_lon": 36.2, + "distance_m": 11.1, + "threshold_m": 200.0, + }, + ), + ( + "c5.gps_bounded_delta.reject", + { + "sample_lat": 50.0, + "sample_lon": 36.2, + "smoother_lat": 50.005, + "smoother_lon": 36.205, + "distance_m": 700.0, + "threshold_m": 200.0, + }, + ), + ], +) +def test_ac12_fdr_kinds_round_trip(kind: str, payload: dict[str, Any]) -> None: + record = FdrRecord( + schema_version=1, + ts=datetime.now(tz=timezone.utc).isoformat(), + producer_id="c5_state", + kind=kind, + payload=payload, + ) + + encoded = serialise(record) + decoded = parse(encoded) + + assert decoded.kind == kind + assert decoded.payload == payload + # No unknown-key bucket for the AZ-490 shape (every key is registered) + assert "extra" not in decoded.payload + + +# --------------------------------------------------------------------- +# AC-13: No origin → FC-EKF cold-start path emits one fc_ekf record. + + +def test_ac13_isam2_no_origin_emits_fc_ekf_record_on_first_add() -> None: + estimator = _build_isam2() + estimator._isam2_handle = mock.MagicMock() + + estimator.add_vio(_vio(frame_id=1, t_seconds=1.0)) + estimator.add_vio(_vio(frame_id=2, t_seconds=2.0)) + + records = _cold_start_records(estimator) + assert len(records) == 1 + assert records[0].payload["source"] == "fc_ekf" + + +def test_ac13_eskf_no_origin_emits_fc_ekf_record_on_first_add() -> None: + estimator = _build_eskf() + + estimator.add_vio(_vio(frame_id=1, t_seconds=1.0)) + estimator.add_vio(_vio(frame_id=2, t_seconds=2.0)) + + records = _cold_start_records(estimator) + assert len(records) == 1 + assert records[0].payload["source"] == "fc_ekf" + + +# --------------------------------------------------------------------- +# AC-14: AZ-385's first two clauses are unchanged — bounded-delta is additive. +# +# We re-run the spoof-gate engagement / gate-lift path WITHOUT touching +# the bounded-delta method and assert the canonical gate behaviour +# still holds. The full AZ-385 acceptance suite re-runs in +# test_az385_source_label_spoof_gate.py — this test is the smoke +# check that the new clause did not perturb the existing wiring. + + +def test_ac14_existing_spoof_gate_unchanged_by_new_clause() -> None: + from gps_denied_onboard._types.fc import GpsHealth, GpsStatus + + clock = _Clock(0) + sm, _ = _make_sm(bounded_delta_m=200.0, clock=clock) + + # Initial — DEAD_RECKONED until first anchor + assert sm.current_label() == PoseSourceLabel.DEAD_RECKONED + + # Anchor without a spoof event → SATELLITE_ANCHORED + sm.notify_gps_health( + GpsHealth(status=GpsStatus.STABLE_NON_SPOOFED, fix_age_ms=10, captured_at=0) + ) + clock.t = 1_000_000_000 # 1 s in + sm.notify_satellite_anchor(now_ns=clock.t, gps_consistency_delta_m=1.0) + assert sm.current_label() == PoseSourceLabel.SATELLITE_ANCHORED + + # Spoof event → gate latches closed → VISUAL_PROPAGATED + sm.notify_gps_health( + GpsHealth(status=GpsStatus.SPOOFED, fix_age_ms=10, captured_at=0) + ) + assert sm.is_spoof_promotion_blocked() is True + assert sm.current_label() == PoseSourceLabel.VISUAL_PROPAGATED + + +# --------------------------------------------------------------------- +# AC-15: Distance is computed via WgsConverter (geodetic), not haversine +# on the equirectangular projection. + + +def test_ac15_geodetic_distance_at_60n_within_half_meter() -> None: + """AC-15 — distance computed via WgsConverter agrees with the WGS-84 + geodesic (pyproj.Geod inverse) within 0.5 m at 60° N for a ~200 m + east-west offset. + + The ground truth is the WGS-84 ellipsoid Vincenty distance from + pyproj's Geod, NOT a haversine-on-equirectangular shortcut — the + AC explicitly excludes the latter shortcut. + """ + from pyproj import Geod + + origin = LatLonAlt(lat_deg=60.0, lon_deg=10.0, alt_m=0.0) + # Construct a sample that is approximately 200 m due east using the + # spherical estimate; the test only requires agreement with the + # ellipsoid Vincenty distance, NOT 200 m exactly. + earth_radius_m = 6_378_137.0 + metres_per_degree_lon = ( + math.pi * earth_radius_m * math.cos(math.radians(60.0)) / 180.0 + ) + sample = LatLonAlt( + lat_deg=60.0, + lon_deg=10.0 + 200.0 / metres_per_degree_lon, + alt_m=0.0, + ) + + geod = Geod(ellps="WGS84") + _az_fwd, _az_back, ground_truth_m = geod.inv( + origin.lon_deg, origin.lat_deg, sample.lon_deg, sample.lat_deg + ) + + distance = WgsConverter.horizontal_distance_m(origin, sample) + + assert abs(distance - ground_truth_m) < 0.5 + # Symmetry — Principle of geodesic invariance. + distance_reverse = WgsConverter.horizontal_distance_m(sample, origin) + assert abs(distance - distance_reverse) < 1e-6 + + +# --------------------------------------------------------------------- +# Bounded-delta gate ignores no-smoother case. + + +def test_bounded_delta_no_smoother_returns_none() -> None: + sm, fdr = _make_sm(bounded_delta_m=200.0) + sample = LatLonAlt(lat_deg=50.0, lon_deg=36.0, alt_m=200.0) + + result = sm.process_gps_sample(sample, smoother_estimate=None) + + assert result is None + # No FDR record either way + assert fdr.enqueue.call_count == 0 + + +# --------------------------------------------------------------------- +# Estimator.notify_gps_sample — happy-path delegation. + + +def test_eskf_notify_gps_sample_delegates_to_state_machine() -> None: + # Arrange — seed an operator origin so the smoother latlon is real + estimator = _build_eskf() + estimator.set_takeoff_origin(_origin(), sigma_horiz_m=5.0, sigma_vert_m=10.0) + + # Sample within the 200 m ring of the origin + sample = GpsSample( + position_wgs84=LatLonAlt( + lat_deg=_origin().lat_deg + 0.0008, + lon_deg=_origin().lon_deg + 0.0008, + alt_m=200.0, + ), + captured_at=1_000_000_000, + ) + + # Act + result = estimator.notify_gps_sample(sample) + + # Assert + assert result == BOUNDED_DELTA_SOFT + accepts = _bounded_delta_records( + estimator._fdr_client, kind="c5.gps_bounded_delta.accept" + ) + assert len(accepts) == 1 diff --git a/tests/unit/test_az272_fdr_record_schema.py b/tests/unit/test_az272_fdr_record_schema.py index 77f78ca..40c91d2 100644 --- a/tests/unit/test_az272_fdr_record_schema.py +++ b/tests/unit/test_az272_fdr_record_schema.py @@ -93,6 +93,36 @@ def _kind_payload(kind: str) -> dict[str, object]: "rollover_count": 0, "clean_shutdown": True, } + # AZ-490 / E-C5: operator-origin cold-start ladder + bounded-delta GPS gate. + if kind == "c5.cold_start_origin.set": + return { + "source": "manifest", + "lat_deg": 50.0, + "lon_deg": 36.2, + "alt_m": 200.0, + "sigma_horiz_m": 5.0, + "sigma_vert_m": 10.0, + } + if kind == "c5.cold_start_origin.unavailable": + return {"reason": "no_manifest_origin_no_fc_ekf"} + if kind == "c5.gps_bounded_delta.accept": + return { + "sample_lat": 50.0, + "sample_lon": 36.2, + "smoother_lat": 50.0001, + "smoother_lon": 36.2, + "distance_m": 11.1, + "threshold_m": 200.0, + } + if kind == "c5.gps_bounded_delta.reject": + return { + "sample_lat": 50.0, + "sample_lon": 36.2, + "smoother_lat": 50.005, + "smoother_lon": 36.205, + "distance_m": 700.0, + "threshold_m": 200.0, + } raise AssertionError(f"unhandled kind in fixture: {kind!r}")