# C4 D-CROSS-LATENCY-1 Hybrid — Jacobian fallback + thermal-state-driven mode switch

**Task**: AZ-361_c4_jacobian_thermal_hybrid
**Name**: C4 D-CROSS-LATENCY-1 hybrid — Jacobian-degraded covariance + per-frame thermal-state-driven mode switch
**Description**: Extend `OpenCVGtsamPoseEstimator` (TBD AZ-? Marginals task) with the D-CROSS-LATENCY-1 hybrid: when `thermal_state.throttle == True`, REPLACE the `Marginals.marginalCovariance(pose_key)` path with a Jacobian-derived 6×6 covariance computed directly from the OpenCV `solvePnPRansac` outputs (`rvec`, `tvec`, inlier residuals) using the standard PnP Jacobian + reprojection-residual variance. The pose estimate itself is still `solvePnPRansac` output; ONLY the covariance recovery path differs. The mode-switch decision is made PER FRAME at the start of `estimate(...)` based on `thermal_state.throttle`. Switching MARGINALS → JACOBIAN or back happens immediately on the next call (Invariant 4 — mode-switch latency ≤ 1 frame). The Jacobian path also emits `CovarianceDegradedWarning` via `warnings.warn(...)` once per 60 s window (filterwarnings-based rate-limiting). Replaces the AZ-? Marginals task's `NotImplementedError("Jacobian path owned by Hybrid task...")` branch with the actual implementation.
**Complexity**: 3 points
**Dependencies**: AZ-358 (Marginals path + class scaffold), AZ-355 (`CovarianceDegradedWarning` + `CovarianceMode` enum), AZ-302_c7_thermal_publisher (`ThermalState` source), AZ-277_se3_utils, AZ-279_wgs_converter, AZ-269_config_loader, AZ-266_log_module, AZ-272_fdr_record_schema, AZ-263
**Component**: c4_pose (epic AZ-259 / E-C4)
**Tracker**: AZ-361
**Epic**: AZ-259 (E-C4)

### Document Dependencies

- `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md` — Hybrid task scope.
- `_docs/02_document/components/06_c4_pose/description.md` — § 1 D-C4-2 = (a) Jacobian fallback under throttle; § 5 `CovarianceDegradedWarning` semantics; § 7 ~5–15 ms Jacobian cost.
- `_docs/02_document/components/06_c4_pose/tests.md` — C4-IT-03 (D-CROSS-LATENCY-1 mode switch within 1 frame), C4-PT-01 (JACOBIAN p95 ≤ 15 ms).
- `_docs/02_document/architecture.md` — ADR-006 (~5–10% accuracy loss accepted under throttle), R10.

## Problem

Without this task, the system has only the Marginals path; under thermal throttle (sustained Jetson high-temperature mode), `Marginals.marginalCovariance(pose_key)` consumes ~60 ms of the AC-4.1 latency budget AND GPU/CPU contention with C7 inference makes the overall pipeline miss the 400 ms p95 budget. The D-CROSS-LATENCY-1 hybrid trades ~5–10% covariance accuracy (per ADR-006) for ~5–15 ms Jacobian-path latency, restoring the latency budget. AC-NEW-5 (operating envelope; thermal-throttle-driven covariance degradation hybrid) requires this path to exist and to switch per-frame within 1 frame of the thermal flag flipping.

## Outcome

- `src/gps_denied_onboard/components/c4_pose/opencv_gtsam_estimator.py` — REMOVE the `NotImplementedError` branch; ADD the Jacobian path.
- New private method `_estimate_marginals_path(...)` extracted from the existing Marginals body (refactor; no behaviour change).
- New private method `_estimate_jacobian_path(...)`:
  1. Run `cv2.solvePnPRansac` (same call as Marginals path) — pose extraction is identical.
  2. From the inlier 2D points + 3D world points + reprojected coordinates, compute the per-point reprojection residuals.
  3. Compute the PnP Jacobian `J` (6×N) at the converged pose using OpenCV's `cv2.projectPoints` with derivative output (or via `SE3Utils.numerical_jacobian` if OpenCV does not expose the analytical Jacobian for IPPE).
  4. Compute residual variance σ² as `mean(residuals²)` (isotropic noise model — documented as the simplification the Jacobian path makes per ADR-006).
  5. Compute the 6×6 information matrix `Λ = (1/σ²) · Jᵀ J`.
  6. Compute covariance `Σ = inv(Λ + ε·I)` with ε = 1e-9 (ridge regularisation for numerical stability when `Λ` is ill-conditioned).
  7. Verify SPD: `np.linalg.cholesky(Σ)` succeeds; on failure raise `PnpFailureError("non-SPD Jacobian covariance; numerical instability")` (defensive).
  8. Convert pose to WGS84 via `WgsConverter.local_to_wgs84(pose)` — SAME conversion path as Marginals.
  9. Assemble `PoseEstimate(..., covariance_mode = JACOBIAN, source_label = SATELLITE_ANCHORED)`.
  10. `self._last_covariance_mode = JACOBIAN`.
  11. **`warnings.warn(CovarianceDegradedWarning("Jacobian covariance engaged; thermal_throttle=true"), stacklevel=2)`** — first time per 60 s window; rate-limited via `_jacobian_warn_window` (timestamp of last emission). Subsequent invocations within the window use `warnings.simplefilter("once")` semantics — emit only the first one.
  12. WARN log `kind="c4.pose.covariance_degraded"` with `{frame_id, thermal_state}` — emitted ONCE per 60 s window (rate-limited).
  13. FDR `pose.frame_done` record with `mode: "jacobian"`.
- `estimate(...)` becomes a dispatcher: read `thermal_state.throttle` at entry; call `_estimate_marginals_path(...)` or `_estimate_jacobian_path(...)`. NO state buffering — strictly per-call.
- Note: the Jacobian path does NOT add a factor to C5's iSAM2 graph (per design — under throttle, the system runs lighter; C5 receives a `PoseEstimate` but no graph factor add). DOCUMENT this trade explicitly: under throttle, the iSAM2 graph stops growing; recovery happens automatically when the thermal flag flips back.

> **Cross-task interaction with AZ-260 (C5)**. C5's iSAM2 update path needs to handle the case where C4 emits a `PoseEstimate` without a corresponding factor add (Jacobian path). C5's `add_pose_anchor(pose_estimate)` MUST inspect `pose_estimate.covariance_mode` and skip the graph-resync work for `JACOBIAN`. This requirement is captured in AZ-260's task spec; this task does NOT modify C5 — it only documents the requirement.

## Scope

### Included
- Refactor of existing `OpenCVGtsamPoseEstimator.estimate(...)` into a dispatcher + two private path methods.
- New `_estimate_jacobian_path(...)` implementation.
- Per-frame thermal-state-driven dispatch.
- `CovarianceDegradedWarning` emission via `warnings.warn` (NOT raise).
- 60 s rate-limiting on the WARN log AND the warnings.warn emission.
- SPD-invariant defensive check on Jacobian covariance.
- Removal of `NotImplementedError` from AZ-? Marginals task's body.
- Documentation update to the contract / description: explicit note that the Jacobian path does NOT add to C5's iSAM2 graph.
- Unit tests covering: thermal flag flip → mode switch within 1 frame; Jacobian covariance is SPD; Jacobian covariance produces accuracy WITHIN ~5–10% of Marginals on a synthetic baseline; `CovarianceDegradedWarning` emitted via `warnings.warn` not raise; rate-limiting of the warning + WARN log.

### Excluded
- The Marginals path — already shipped by AZ-?.
- C5's iSAM2 update-path adjustment (`add_pose_anchor` mode inspection) — owned by AZ-260; this task only documents the requirement.
- The `ThermalState` source — owned by AZ-302.
- C4-IT-03 + C4-PT-01 component-internal acceptance tests — deferred to E-BBT (AZ-262); unit tests in this task cover the per-method invariants.

## Acceptance Criteria

**AC-1: Per-frame mode dispatch**
Given an alternating `thermal_state.throttle` sequence (False, True, False, True, ...) over 10 frames
When `estimate(...)` is called for each frame
Then `current_covariance_mode()` returns the correct mode after EACH call (no buffering, no hysteresis).

**AC-2: Mode-switch latency ≤ 1 frame (Invariant 4)**
Given the thermal flag flips between two consecutive `estimate(...)` calls
When the second call runs
Then the new mode IS the new flag's mode (no carry-over from the previous call).

**AC-3: Jacobian covariance is SPD**
Given a successful Jacobian path run
Then `np.linalg.cholesky(covariance_6x6)` succeeds; the matrix is symmetric to 1e-10 tolerance.

**AC-4: `covariance_mode == JACOBIAN` on Jacobian path**
When the Jacobian path runs successfully
Then `PoseEstimate.covariance_mode == CovarianceMode.JACOBIAN` AND `current_covariance_mode() == JACOBIAN`.

**AC-5: `source_label == SATELLITE_ANCHORED` on success regardless of path (Invariant 7)**
Both Marginals and Jacobian paths emit `SATELLITE_ANCHORED` on success.

**AC-6: `CovarianceDegradedWarning` emitted via `warnings.warn`**
When the Jacobian path runs
Then `warnings.warn(CovarianceDegradedWarning("..."))` is called; verified via `warnings.catch_warnings()` test harness. The warning is NOT raised as an exception.

**AC-7: `warnings.warn` rate-limited to ONE per 60 s window**
Given the Jacobian path runs at 3 Hz for 70 s (total 210 calls)
When the warnings emitted are counted
Then exactly 2 warnings are emitted (one for window 0–60 s, one for window 60–120 s).

**AC-8: WARN log rate-limited similarly**
Same scenario as AC-7 → exactly 2 WARN log records `kind="c4.pose.covariance_degraded"`.

**AC-9: Marginals path unchanged by refactor**
Given a Marginals-only test fixture (already exercised in AZ-?)
When the AZ-? tests are re-run after this task's refactor
Then ALL AZ-? AC-1..AC-11 still pass without modification.

**AC-10: Non-SPD Jacobian covariance defensive raise**
Given an inlier set producing a near-singular `JᵀJ` (synthetically degenerate)
When the Jacobian path runs
Then `PnpFailureError("non-SPD Jacobian covariance; numerical instability")` is raised; ERROR log emitted.

**AC-11: Jacobian accuracy within ~5–10% of Marginals on synthetic baseline**
Given 100 synthetic frames AND ground-truth pose
When BOTH paths are run on the same input (test harness toggles `thermal_state.throttle`)
Then the Jacobian path's RMSE is within 1.10× the Marginals path's RMSE (10% tolerance per ADR-006). Informational; does NOT block this AC if the actual ratio is between 1.0 and 1.10.

**AC-12: Jacobian path skips iSAM2 factor add**
Given the Jacobian path runs
Then NO `isam2_graph_handle.add_factor(...)` call AND NO `isam2_graph_handle.update()` call is made; only `last_anchor_age_ms()` is read (for the `last_satellite_anchor_age_ms` field).

**AC-13: FDR `pose.frame_done` distinguishes path**
The `mode` field in the FDR record matches the path: `"marginals"` or `"jacobian"`.

## Non-Functional Requirements

**Performance**
- `_estimate_jacobian_path` p95 ≤ 15 ms target / 25 ms hard limit (per C4-PT-01).
- Mode-switch latency: zero — the dispatch is a single `if` at call entry.

**Compatibility**
- OpenCV ≥ 4.12.0 (same as AZ-?).
- The Jacobian computation MUST work for both analytical (if OpenCV exposes it for IPPE) AND numerical (`SE3Utils.numerical_jacobian`) paths; choose analytical when available, fall back to numerical.

**Reliability**
- SPD-invariant defensive check covers the Jacobian path.
- `CovarianceDegradedWarning` rate-limiting prevents log flooding under sustained throttle.

## Unit Tests

| AC Ref | What to Test | Required Outcome |
|--------|--------------|------------------|
| AC-1 | Alternating thermal flag → alternating mode | Each call's mode matches the flag |
| AC-2 | Mode-switch within 1 frame | New mode on the next call |
| AC-3 | Jacobian SPD | Cholesky succeeds |
| AC-4 | Jacobian mode reporting | Both `PoseEstimate` and `current_covariance_mode()` |
| AC-5 | Source label always SATELLITE_ANCHORED | Both paths |
| AC-6 | `warnings.warn` not raise | Captured via `warnings.catch_warnings()` |
| AC-7 | Warning rate-limited | 2 warnings over 70 s |
| AC-8 | WARN log rate-limited | 2 records over 70 s |
| AC-9 | AZ-? tests still pass | Re-run all 11 ACs after refactor |
| AC-10 | Near-singular Jacobian → defensive raise | `PnpFailureError` |
| AC-11 | Jacobian within 10% of Marginals | RMSE ratio ≤ 1.10 |
| AC-12 | Jacobian skips iSAM2 add | No `add_factor`/`update` calls |
| AC-13 | FDR mode field | `"marginals"` or `"jacobian"` |

## Constraints

- **Single-threaded** by contract; same thread as C5 + the Marginals task.
- **No buffering** — mode dispatch is per-call; thermal flag is read at call entry.
- **Jacobian path skips iSAM2 factor add** — design choice per C5's degraded mode requirement.
- **`CovarianceDegradedWarning` is emitted via `warnings.warn`** — NEVER raised.
- **Rate-limiting is 60 s** for both the warning and the WARN log.
- **Refactor of Marginals path MUST be behaviour-preserving** — AZ-?'s tests pass unchanged (AC-9).

## Risks & Mitigation

**Risk 1: OpenCV may not expose an analytical Jacobian for `SOLVEPNP_IPPE`**
- *Mitigation*: implementation falls back to `SE3Utils.numerical_jacobian` (forward-difference, 1e-6 step). Numerical Jacobian costs ~5 ms additional but stays within the 15 ms target.

**Risk 2: Jacobian-derived covariance is overly optimistic on hard frames**
- *Mitigation*: ADR-006 documents the ~5–10% accuracy loss; AC-11 verifies within tolerance. AC-NEW-5 (full operating envelope verification) belongs to E-BBT / NFT-LIM-04 — workstation-baseline portion only here.

**Risk 3: The `_jacobian_warn_window` rate-limiting might miss a warning if the first frame after the 60 s rollover is a Marginals frame**
- *Mitigation*: window is reset on every Jacobian call; rollover boundaries are not special-cased. The intent is "at least one warning per active throttle window of duration ≥ 60 s", which is satisfied.

**Risk 4: Refactoring AZ-?'s `estimate(...)` may introduce subtle behaviour changes**
- *Mitigation*: AC-9 mandates AZ-?'s full test suite passes unchanged. The refactor is mechanical (extract method); review hook ensures fidelity.

## Runtime Completeness

- **Named capability**: D-CROSS-LATENCY-1 hybrid — Jacobian fallback + per-frame thermal-driven mode switch.
- **Production code that must exist**: real Jacobian computation (analytical or numerical); real per-frame thermal-driven dispatch; real SPD defensive check; real `warnings.warn` emission with rate-limiting; real WARN log emission with rate-limiting; refactor of Marginals path into a private method; FDR record `mode` field distinguishes paths.
- **Allowed external stubs**: `FakeISam2GraphHandle`, `FakeWgsConverter`, `FakeSE3Utils`, `FakeFdrClient` — same as AZ-?.
- **Unacceptable substitutes**: a Marginals call with a "skip the cubic part" hack (would not deliver the latency budget); raising `CovarianceDegradedWarning` instead of emitting via `warnings.warn` (changes the user-facing semantics); skipping the rate limiter (would flood logs under sustained throttle); buffering thermal state across frames (would violate Invariant 4); emitting warnings inside the Marginals path "for symmetry" (no — the warning is documentation that the system is in degraded mode).

## Contract

This task implements the Jacobian / hybrid portion of `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md`.
After this task lands, the contract's invariants (especially Invariants 3, 4, 6, 9) are FULLY exercised.
