mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 22:01:13 +00:00
[AZ-490] C5 set_takeoff_origin entrypoint + bounded-delta GPS gate
Add operator warm-start path to C5 StateEstimator Protocol and both
implementations (GtsamIsam2StateEstimator, EskfStateEstimator), plus
the third clause of the AZ-385 spoof-promotion gate.
- StateEstimator Protocol: set_takeoff_origin(origin, sigma_horiz_m,
sigma_vert_m) -> None.
- iSAM2: PriorFactorPose3 at origin with diagonal sigmas, single
isam2.update().
- ESKF: zero _nominal_pos, overwrite _P position block with sigma**2.
- SourceLabelStateMachine.process_gps_sample bounded-delta clause:
WgsConverter.horizontal_distance_m vs smoother estimate; reject
resets the dwell-time counter so AZ-385 cannot re-promote off bad
GPS.
- New EstimatorAlreadyStartedError (StateEstimatorConfigError
subclass) on late call after first add_*.
- C5StateConfig: spoof_promotion_bounded_delta_m=200,
default_takeoff_origin_sigma_horiz_m=5,
default_takeoff_origin_sigma_vert_m=10.
- New GpsSample DTO + WgsConverter.horizontal_distance_m helper.
- 4 new FDR kinds (cold_start_origin.{set,unavailable},
gps_bounded_delta.{accept,reject}) registered in AZ-272 schema.
- 33 new unit tests cover AC-1..AC-15; full repo 750 passed / 2
skipped (pre-existing CI tooling skips).
Docs synced: protocol contract, C5 component description,
architecture, glossary, system-flows, C10 provisioning description.
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -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]
|
||||
```
|
||||
Reference in New Issue
Block a user