# C5 set_takeoff_origin entrypoint — accept operator origin from C10 Manifest

**Task**: AZ-490_c5_set_takeoff_origin
**Name**: C5 set_takeoff_origin entrypoint — accept operator origin from C10 Manifest
**Description**: Extend `StateEstimator` (Protocol + both concrete impls — `GtsamIsam2StateEstimator` and `EskfStateEstimator`) with a new pre-takeoff entrypoint `set_takeoff_origin(origin: LatLonAlt, sigma_horiz_m: float, sigma_vert_m: float) -> None` that seeds the cold-start prior to the first frame. The composition root calls this method during F2 (Takeoff load) when the C10 ManifestVerifier reports a valid `flight.takeoff_origin`. With operator origin set, the FC-EKF cold-start path becomes the secondary fallback (ADR-010). The method also makes the spoof-promotion gate (AZ-385) consult a third bounded-delta clause: mid-flight FC GPS samples within 200 m of the current smoother estimate may be admitted as a soft constraint; samples > 200 m off are rejected and emit an FDR `c5.gps_bounded_delta.reject` record. The 200 m threshold is config-driven (`spoof_promotion_bounded_delta_m`, default 200.0). This task delivers the C5-side contract + the FDR record kind + the unit tests covering primary/secondary/conflict paths; downstream wiring into the composition root and the F2 sequence is the consumer's responsibility (AZ-381 owner already plumbs it).
**Complexity**: 3 points
**Dependencies**: AZ-381 (Protocol + DTOs + factory), AZ-383 (factor adds), AZ-384 (marginals + outputs), AZ-385 (source-label gate; bounded-delta is a new clause inside its state machine), AZ-386 (ESKF baseline must honour the same entrypoint), AZ-272 (FdrRecord Schema — new `c5.cold_start_origin.set` and `c5.gps_bounded_delta.{accept,reject}` kinds), AZ-273 (FdrClient), AZ-279 (WgsConverter for the bounded-delta Vincenty distance), AZ-269 (config), AZ-266 (logging), AZ-263 (initial structure)
**Component**: c5_state (epic AZ-260 / E-C5)
**Tracker**: AZ-490
**Epic**: AZ-260 (E-C5)

### Document Dependencies

- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` § Invariants 11–12, § Config schema, § Test expectations.
- `_docs/02_document/components/07_c5_state/description.md` § State management (cold-start ladder), § Spoof-promotion gate (3rd clause).
- `_docs/02_document/architecture.md` ADR-010 (operator origin primary), Principle #11 (amended).
- `_docs/02_document/system-flows.md` F2 (Takeoff load), F7 (spoof gate).

## Problem

Today, C5's `StateEstimator` Protocol has only one cold-start trust anchor: the FC EKF GPS snapshot consumed at first frame. ADR-010 makes the operator-planned mission the primary anchor and the FC EKF the secondary fallback — but the C5 Protocol carries no method to accept an external operator origin, so the composition root has nowhere to deliver the Manifest-resolved value.

Concretely:

- The C5 Protocol exposes `add_vio`, `add_pose_anchor`, `add_fc_imu`, `query` — none of which fit "pre-takeoff, set the absolute reference frame". Trying to overload `add_pose_anchor` would conflate "constant absolute origin from a trusted offline source" with "noisy per-frame satellite anchor"; the noise model, gating, and FDR record kind are all different.
- AZ-385's spoof-promotion gate has two clauses (consistency-with-VPR-anchors + dwell-time). ADR-010 amends Principle #11 with a third clause: an FC GPS sample within `spoof_promotion_bounded_delta_m` of the current smoother estimate is admitted as a soft constraint, but samples outside that ring are rejected and counted against the spoof-promotion gate. Without this third clause, mid-flight FC GPS is either fully trusted or fully ignored — losing a legitimate fallback signal when GPS recovers.
- The cold-start ladder is undocumented in the running code: operator origin should win when both are available, FC EKF should win when operator origin is absent, and the system should refuse takeoff when neither is available — but no method captures these states.
- Two concrete estimators (iSAM2 + ESKF) need to honour the same entrypoint with the same semantics so the composition root can switch strategies without re-wiring F2.

This task lands the C5-side contract, the FDR records, and the spoof-gate amendment. It does NOT touch the C10 Manifest parsing (AZ-324), the C12 Flight resolution (AZ-489), or the composition-root F2 wiring (AZ-381 owner sequences that as a follow-up commit).

## Outcome

- **`StateEstimator` Protocol** (`src/gps_denied_onboard/components/c5_state/interface.py`):
  - New method `set_takeoff_origin(origin: LatLonAlt, sigma_horiz_m: float, sigma_vert_m: float) -> None`.
  - Contract: idempotent if called twice with identical args; raises `EstimatorAlreadyStartedError` if called after the first `add_vio` / `add_pose_anchor`; raises `EstimatorConfigError` on negative sigmas or on `LatLonAlt` outside WGS-84 bounds.
- **`GtsamIsam2StateEstimator`** (`src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py`):
  - `set_takeoff_origin(...)` sets the local-ENU origin via `WgsConverter`, seeds the iSAM2 prior factor at `Pose3(Rot3.Identity(), Point3(0,0,0))` with a horizontal sigma of `sigma_horiz_m` and a vertical sigma of `sigma_vert_m`, and emits an FDR `c5.cold_start_origin.set` record (`source="manifest"`) with the origin lat/lon/alt.
  - Internal `_origin_source: Literal["manifest", "fc_ekf", None]` field tracks the cold-start path; first `add_vio` call without an origin set is allowed (FC EKF path stays); first `add_vio` call with an origin set fixes the cold-start ladder to "manifest".
- **`EskfStateEstimator`** (`src/gps_denied_onboard/components/c5_state/eskf_baseline.py`):
  - Same method, same semantics; for ESKF the origin is set on the local-ENU converter; the state's nominal position prior is set to `(0,0,0)` with covariance `diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)`.
- **Spoof-promotion gate amendment** (`src/gps_denied_onboard/components/c5_state/source_label_state_machine.py` — owned by AZ-385, this task patches it with one new clause):
  - When the gate processes an incoming FC GPS sample, it computes `vincenty(current_smoother_latlon, sample_latlon)` via `WgsConverter`.
  - If `distance ≤ spoof_promotion_bounded_delta_m`: sample is admitted as a `BOUNDED_DELTA_SOFT` source label; FDR `c5.gps_bounded_delta.accept` is emitted.
  - If `distance > spoof_promotion_bounded_delta_m`: sample is rejected; FDR `c5.gps_bounded_delta.reject` is emitted naming the sample, the current estimate, and the computed distance; the rejection counts against the existing dwell-time clause.
- **Config schema additions** (`src/gps_denied_onboard/config/c5_state.py` via AZ-269's loader):
  - `spoof_promotion_bounded_delta_m: float` (default `200.0`).
  - `default_takeoff_origin_sigma_horiz_m: float` (default `5.0`) — used when the Manifest does not carry an explicit sigma.
  - `default_takeoff_origin_sigma_vert_m: float` (default `10.0`).
- **FDR record kinds** (`src/gps_denied_onboard/fdr/record_schema.py` via AZ-272 schema extension):
  - `c5.cold_start_origin.set` — `{source: "manifest" | "fc_ekf", lat_deg, lon_deg, alt_m, sigma_horiz_m, sigma_vert_m}`.
  - `c5.cold_start_origin.unavailable` — emitted when neither anchor is available; carries a takeoff-abort reason code.
  - `c5.gps_bounded_delta.accept` — `{sample_lat, sample_lon, smoother_lat, smoother_lon, distance_m, threshold_m}`.
  - `c5.gps_bounded_delta.reject` — same shape as `accept`.
- **Logging**:
  - INFO on every `set_takeoff_origin` call (`kind="c5.cold_start_origin.set"`).
  - WARN on every bounded-delta reject (`kind="c5.gps_bounded_delta.reject"`).

## Scope

### Included

- Protocol method addition + both concrete impls (iSAM2 + ESKF).
- WgsConverter integration for ENU origin + Vincenty distance in bounded-delta gate.
- Spoof-gate's third clause + FDR record emission.
- Config schema entries for the three new keys.
- Error classes: `EstimatorAlreadyStartedError`, `EstimatorConfigError`.
- Unit tests covering every AC: idempotency, post-start rejection, sigma validation, lat/lon bounds, bounded-delta accept/reject, source-label transition, FDR emission shape, both estimator impls.

### Excluded

- C10 Manifest schema changes (owned by AZ-323 / AZ-324).
- C12 Flight resolution (owned by AZ-489).
- Composition-root F2 wiring of `manifest.takeoff_origin → estimator.set_takeoff_origin(...)` (owned by AZ-381 owner as a follow-up commit, NOT this task).
- Operator origin "refresh" mid-flight (out of scope — the cold-start anchor is set once at takeoff and not revised).
- Changes to the AZ-385 source-label state-machine's first two clauses (consistency + dwell-time); only the third clause is added here.
- ESKF deep-rewrite (the ESKF stays as the AZ-386 baseline; only `set_takeoff_origin` + bounded-delta admission are added).

## Acceptance Criteria

**AC-1: Protocol-conformance — both impls expose `set_takeoff_origin`**
Given `from c5_state import GtsamIsam2StateEstimator, EskfStateEstimator, StateEstimator`
When `isinstance(impl, StateEstimator)` is checked on each
Then both return `True`; both have a callable `set_takeoff_origin` with the documented signature.

**AC-2: Happy path — origin set before first VIO seeds the smoother prior (iSAM2)**
Given a fresh `GtsamIsam2StateEstimator`
When `set_takeoff_origin(LatLonAlt(50.0, 36.2, 200.0), sigma_horiz_m=5.0, sigma_vert_m=10.0)` is called
Then the iSAM2 graph has exactly one prior factor at `Pose3.Identity()` with sigma diag matching `[deg2rad(5°)*3, deg2rad(5°)*3, deg2rad(5°)*3, 5.0, 5.0, 10.0]` (rotation sigma is the iSAM2 default Identity prior; translation sigmas come from the call); ONE FDR record `c5.cold_start_origin.set` is emitted with `source="manifest"`; ONE INFO log.

**AC-3: Happy path — origin set before first VIO seeds the ESKF state prior**
Given a fresh `EskfStateEstimator`
When `set_takeoff_origin(LatLonAlt(50.0, 36.2, 200.0), sigma_horiz_m=5.0, sigma_vert_m=10.0)` is called
Then the ESKF's `P` matrix's position block is `diag(25.0, 25.0, 100.0)`; the ENU origin is set to the call's `LatLonAlt`; ONE FDR record + ONE INFO log as in AC-2.

**AC-4: Idempotent — calling twice with identical args is a no-op**
Given the estimator after a first `set_takeoff_origin(A, s, s_v)` call
When `set_takeoff_origin(A, s, s_v)` is called again with identical args
Then no second FDR record is emitted; no second prior factor is added; no exception raised.

**AC-5: Conflict — calling twice with different args raises `EstimatorConfigError`**
Given the estimator after `set_takeoff_origin(A, ...)`
When `set_takeoff_origin(B, ...)` is called with `A != B`
Then `EstimatorConfigError` is raised; message names both values; the estimator state is unchanged.

**AC-6: Late call — `set_takeoff_origin` after first `add_vio` raises `EstimatorAlreadyStartedError`**
Given the estimator after one `add_vio(...)` call
When `set_takeoff_origin(...)` is called
Then `EstimatorAlreadyStartedError` is raised; the estimator state is unchanged.

**AC-7: Bounds — invalid `LatLonAlt` raises `EstimatorConfigError`**
Given `LatLonAlt(lat_deg=95.0, ...)` (out of WGS-84 bounds)
When `set_takeoff_origin(invalid_origin, ...)` is called
Then `EstimatorConfigError` is raised; message names the violated bound.

**AC-8: Negative sigma raises `EstimatorConfigError`**
Given `sigma_horiz_m=-5.0`
When `set_takeoff_origin(...)` is called
Then `EstimatorConfigError`; message names the violated invariant.

**AC-9: Bounded-delta accept — incoming GPS within 200 m of smoother estimate is admitted**
Given a running smoother with current estimate at `LatLonAlt(50.000, 36.200, 200.0)`
When a `GpsSample(LatLonAlt(50.0008, 36.2008, 200.0))` arrives via the existing AZ-391 inbound path (distance ≈ 110 m at 50° N)
Then the gate admits the sample with source label `BOUNDED_DELTA_SOFT`; ONE FDR record `c5.gps_bounded_delta.accept` is emitted; the sample contributes to the iSAM2 graph as a soft factor with sigma per the config; the source-label state machine's first-two-clause behaviour is unchanged.

**AC-10: Bounded-delta reject — incoming GPS > 200 m off is rejected**
Given the same smoother as AC-9
When a `GpsSample(LatLonAlt(50.005, 36.205, 200.0))` arrives (distance ≈ 700 m)
Then the gate rejects the sample; ONE FDR `c5.gps_bounded_delta.reject` record is emitted naming sample, smoother estimate, and distance; the sample is NOT added to the graph; the rejection increments the source-label state machine's dwell-time counter.

**AC-11: Threshold is config-driven — setting `spoof_promotion_bounded_delta_m=500.0` admits AC-10's sample**
Given the config override `spoof_promotion_bounded_delta_m=500.0` is in effect (NOTE: 500 m is still < AC-10's 700 m, so the assertion is: the original AC-10 reject still happens, but with a 500 m threshold a 300 m offset now passes)
Re-spec to:
Given the config override `spoof_promotion_bounded_delta_m=1000.0` is in effect
When the AC-10 sample arrives
Then it is admitted as `BOUNDED_DELTA_SOFT` (it now sits within the relaxed ring).

**AC-12: FDR record kinds are registered in the AZ-272 schema**
Given the AZ-272 schema after this task
When `kind="c5.cold_start_origin.set"`, `kind="c5.cold_start_origin.unavailable"`, `kind="c5.gps_bounded_delta.accept"`, `kind="c5.gps_bounded_delta.reject"` are encoded
Then each round-trips through serialization without raising; the schema contract test (AZ-268) covers all four.

**AC-13: No-op when no origin is set — FC EKF cold-start path unchanged**
Given a fresh estimator
When `add_vio(...)` is called WITHOUT a prior `set_takeoff_origin` call
Then the estimator falls back to the legacy FC EKF cold-start path; the FDR record `c5.cold_start_origin.set` is emitted with `source="fc_ekf"` exactly once at first frame (this is the legacy path, just newly logged); no new behaviour beyond the FDR record kind.

**AC-14: Source-label state machine remains stable for AZ-385's first two clauses**
Given the bounded-delta clause's introduction
When the AZ-385 acceptance tests run unmodified
Then they pass — the bounded-delta clause is additive, not replacing the existing two.

**AC-15: Vincenty distance is computed via `WgsConverter` (not naive haversine on the equirectangular projection)**
Given a smoother estimate at high latitude (`60° N`) and a sample 200 m to the east
When the gate computes the distance
Then it uses `WgsConverter.vincenty_distance` and matches the documented ground-truth distance within ±0.5 m.

## Non-Functional Requirements

**Performance**
- `set_takeoff_origin` wall-clock ≤ 5 ms (single prior factor insertion + one FDR emission).
- Bounded-delta check on every inbound GPS sample ≤ 1 ms (single Vincenty call + threshold compare).

**Reliability**
- Idempotency at AC-4 prevents accidental double-seeding on composition-root retries.
- Late-call rejection at AC-6 prevents bricking an in-flight smoother by mistake.
- All four FDR record kinds are part of the AZ-272 schema; AZ-268's contract test gates schema drift.

**Compatibility**
- Both estimator impls (iSAM2 + ESKF) honour the same signature so the composition root can switch strategies without re-wiring F2.
- The legacy FC EKF cold-start path stays as the secondary fallback (AC-13); existing AZ-419's old AC-3 still passes against the FC EKF path when no Manifest origin is present.

## Unit Tests

| AC Ref | What to Test | Required Outcome |
|--------|-------------|------------------|
| AC-1 | Protocol conformance both impls | `isinstance` True |
| AC-2 | iSAM2 set-origin seeds prior | Prior factor + sigmas; FDR record |
| AC-3 | ESKF set-origin seeds P | P block matches; ENU origin set; FDR record |
| AC-4 | Idempotent double-call | No second FDR; no exception |
| AC-5 | Conflict double-call | `EstimatorConfigError`; names both |
| AC-6 | Late call after add_vio | `EstimatorAlreadyStartedError` |
| AC-7 | Out-of-bounds lat | `EstimatorConfigError` |
| AC-8 | Negative sigma | `EstimatorConfigError` |
| AC-9 | Bounded-delta accept | `BOUNDED_DELTA_SOFT` label + FDR |
| AC-10 | Bounded-delta reject | Sample dropped + FDR reject |
| AC-11 | Threshold config override | AC-10 sample admitted at relaxed threshold |
| AC-12 | FDR schema round-trip | All 4 kinds serialise; AZ-268 covers |
| AC-13 | No origin → FC EKF path | Legacy path + new FDR record `source="fc_ekf"` |
| AC-14 | AZ-385's first 2 clauses unchanged | All AZ-385 tests pass unmodified |
| AC-15 | Vincenty at 60° N | Within ±0.5 m of ground truth |

## Constraints

- `set_takeoff_origin` is the ONLY supported pre-takeoff origin entrypoint; do NOT add `set_takeoff_origin_from_gps(...)` or similar convenience overloads — the FC EKF path stays purely inside the legacy first-frame logic.
- The bounded-delta clause is the THIRD clause of the AZ-385 source-label state machine; do NOT replace AZ-385's existing consistency + dwell-time logic — just add the new clause and its FDR records.
- No clock or `datetime.now()` calls in this code path — pass a `Clock` Protocol per the project's established pattern (used by AZ-273, AZ-382).
- The 200 m threshold is configurable but NOT operator-tunable from the GCS — it's a deploy-time config.
- No "auto-correct origin" or "average origins" logic — operator origin set once at takeoff or not at all.

## Risks & Mitigation

**Risk 1: Operator origin from a stale/wrong flight plan poisons the cold start**
- *Risk*: Mission Planner export drift, manual file edits, or a wrong `--flight-id` flag selects the wrong route.
- *Mitigation*: AZ-323's Manifest carries `flight_id` + `takeoff_origin`, both hashed into `manifest_hash`; AZ-324 validates origin is inside the bbox; this task validates `LatLonAlt` bounds (AC-7) but does NOT re-validate against the Manifest's bbox — the Manifest-level check is sufficient. AC-5's conflict-on-double-call surfaces drift if the composition root somehow re-calls with a different value.

**Risk 2: Bounded-delta gate admits spoofed GPS that happens to be < 200 m off**
- *Risk*: A sophisticated spoofer reads the C5's smoother output and emits GPS within the ring.
- *Mitigation*: Bounded-delta admission is `BOUNDED_DELTA_SOFT`, NOT `SATELLITE_ANCHORED`; AZ-385's other two clauses (consistency-with-VPR-anchors + dwell-time) remain authoritative for the final spoof-promotion decision. The bounded-delta channel is a soft constraint, not a hard reference.

**Risk 3: Composition root forgets to call `set_takeoff_origin` even though the Manifest carries one**
- *Risk*: F2 wiring drift between AZ-381 and AZ-324 — the Manifest has the origin, but the estimator doesn't see it.
- *Mitigation*: AZ-419's updated AC-1 / AC-4 (operator origin test cases) exercise the full F2 chain end-to-end; failure would be visible there. AC-13 here proves the no-origin path is still functional so this task does not break the legacy fallback during the transition.

## Runtime Completeness

- **Named capability**: pre-takeoff operator origin acceptance + mid-flight bounded-delta GPS gate (ADR-010 Principle #11 amended).
- **Production code that must exist**: real `set_takeoff_origin` on both `GtsamIsam2StateEstimator` and `EskfStateEstimator`; real `WgsConverter` for ENU origin + Vincenty distance; real FDR record emission with the four new kinds; real config wiring via AZ-269.
- **Allowed external stubs**: tests use `FakeFdrSink` (AZ-275), a fake `Clock`, and a fixed `WgsConverter` instance with a known ENU origin.
- **Unacceptable substitutes**: a "set-origin" convenience that just sets a class attribute without seeding the prior factor (would silently no-op); haversine instead of Vincenty (loses precision at high lat → AC-15 fails); skipping the FDR records "because they're operational telemetry" (the schema contract test AZ-268 would still flag them as missing).
