gps-denied-onboard/_docs/03_implementation/batch_22_cycle1_report.md

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]