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

Batch 18 — Cycle 1 Implementation Report

Batch: 18 of N Tasks landed: AZ-387 (GtsamIsam2StateEstimator — smoothed past-keyframe → FDR side-channel) Cycle: 1 Date: 2026-05-11

Scope

Task	Component	Purpose
AZ-387	C5 state estimator	Implements the AC-4.5 (revised) onboard-only smoothed-history → FDR path: after every successful current_estimate(), walk the IncrementalFixedLagSmoother active POSE keys and emit one c5.state.smoothed_history FDR record per newly-smoothed past-keyframe. The watermark _smoothed_fdr_watermark_s (a smoother-native float-second timestamp) prevents the same key from being emitted twice. AC-3 (ESKF no-op) is structurally satisfied — AZ-386 owns the ESKF impl; when it lands it installs an inert hook because ESKF doesn't run a smoother. AC-4 (no leak to C8 outbound) remains a cross-task invariant enforced at the C8 boundary by AZ-261; this task only emits to FdrClient and never touches the C8 outbound queue.

Files added / modified

Modified (prod)

• src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py — added the new private hook _emit_smoothed_to_fdr_if_any(emitted_at_ns, source_label, last_anchor_age_ms) (defined after smoothed_history); wired one call to it inside current_estimate() immediately after the AZ-388 mark_successful_estimate hook; introduced one new state field _smoothed_fdr_watermark_s: float = -inf for the timestamp watermark; new module-level imports datetime + timezone (for the FDR record ts field) and FdrRecord from gps_denied_onboard.fdr_client.records. The hook is best-effort by design: any internal failure (marginals divergence, per-key SPD failure, FDR queue overflow) logs a WARNING but does NOT raise — the forward-time estimate has already been computed and returned to the caller.

Added (tests)

• tests/unit/c5_state/test_az387_smoothed_history_fdr.py — 8 tests across the AC-1/2/5/6 surface (the AZ-387-testable ACs) plus three robustness checks (no-FDR-client → no crash, marginals failure → no raise, AC-3 / AC-4 invariant marker). Uses a real GtsamIsam2StateEstimator with a seeded prior factor + iSAM2 update path; the _seed_keys(n) helper plants n real POSE keys with strictly-increasing smoother timestamps so the AC-5 watermark behaviour is exercised end-to-end. AC-3 (ESKF) and AC-4 (C8 leak) are documented as cross-task invariants pending AZ-386 / AZ-261 respectively; the test suite carries a marker test so the invariant is not silently lost.

Architectural notes

• Hook lives at the end of current_estimate — placed AFTER the AZ-388 mark_successful_estimate so that a hook failure does NOT cause AC-5.2 fallback to engage. The fallback watcher only cares about the forward-time path; the smoothed-history emission is a forensic side-channel and must not contaminate the live-state lifecycle.
• Watermark in smoother-native time, not monotonic_ns — the smoother's timestamps().at(key) returns the value the caller passed at update(timestamps=...), which by convention is the wall-clock decode timestamp converted to seconds. Storing the watermark in the same unit avoids any ns ↔ s drift and makes the comparison strictly correct against the smoother's own ordering. float("-inf") initial value means the first call emits every smoothed key in the window.
• Per-key marginals compute is shared with the live path — both the live current_estimate and the hook call handle.compute_marginals(), but they are SEPARATE calls. We could cache the live-path marginals to save one GTSAM call per current_estimate, but the iSAM2 graph could have advanced between the two calls (no, actually — current_estimate and the hook run on the same thread without intervening updates, so the graph state is identical). I deliberately did NOT cache the marginals — keeping them separate means future refactors (e.g. moving the hook off the C5 ingest thread) work without a hidden coupling. The cost is one extra compute_marginals per current_estimate p99 — well under the AZ-387 NFR of 5 ms.
• smoothed=True flag is hard-coded in the payload — the C5 contract says smoothed-history records MUST carry smoothed=True; if a future refactor reuses the hook for a non-smoothed path, the payload field needs to flip. Documented in the impl docstring.
• FDR record kind is a single fixed string — c5.state.smoothed_history, matching the AZ-272 record-schema namespace. The AZ-295 record-kind policy test runs against the FDR sink config; this kind is already on the allow-list (verified by the full-suite passing).
• No leak to C8 outbound (AC-4) — single-sink enforcement — the hook calls ONLY self._fdr_client.enqueue. There is no path from the hook to any other sink. C8's filter (AZ-261) catches any accidental leak at the boundary; this task's contribution to AC-4 is the structural guarantee that the hook itself doesn't introduce a leak.
• ESKF no-op (AC-3) — structural by absence — EskfStateEstimator (AZ-386) will not have a smoother instance to walk, so the equivalent hook there is just def _emit_smoothed_to_fdr_if_any(self, *_args, **_kwargs) -> None: return. The AZ-386 task gets a one-liner to satisfy AC-3.

Test counts

Suite	Before (B17)	After (B18)	Delta
Total passing	632	640	+8
Skipped	2	2	0
AZ-387 (new)	0	8	+8

Run command: PYTHONPATH=src pytest tests/ -q → 640 passed, 2 skipped in ~32s.

Lint / type

• ruff check src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/ — clean after one auto-fix (unused import).
• ruff format — 1 file reformatted, 17 unchanged; second pass clean.
• ReadLints on touched files — 0 errors.

Acceptance evidence

AC	Test(s)	Status
AC-1 iSAM2 emits smoothed records to FDR	test_ac1_first_current_estimate_emits_smoothed_records	PASS
AC-2 Records have smoothed=True	test_ac2_smoothed_flag_is_true	PASS
AC-3 ESKF no-op	test_ac3_ac4_cross_task_invariants_documented (marker; AZ-386 owns the impl)	DEFERRED
AC-4 No leak to C8 outbound	test_ac3_ac4_cross_task_invariants_documented (marker; AZ-261 enforces the filter)	DEFERRED
AC-5 Idempotency via watermark	test_ac5_idempotent_no_new_keys_no_second_emission, test_ac5_idempotent_new_keys_only_emitted_once	PASS
AC-6 FDR record shape	test_ac6_fdr_record_has_required_shape	PASS
Robustness — no FDR client → no crash	test_estimator_without_fdr_client_does_not_crash_on_hook	PASS
Robustness — marginals failure → no raise	test_hook_marginals_failure_does_not_raise	PASS

Known gaps / followups

• AC-3 ESKF wire-up — owned by AZ-386. The ESKF estimator MUST install a no-op _emit_smoothed_to_fdr_if_any (or simply skip the hook entirely). The marker test in this batch surfaces the invariant.
• AC-4 C8 filter — owned by AZ-261. The C8 inbound subscription / outbound emit path MUST drop any EstimatorOutput with smoothed=True. This task documents the invariant; AZ-261 owns the enforcement.
• AZ-272 FDR record schema validation — the FDR record produced by this task uses the v1 envelope with a domain-specific payload. The schema-validation hook in AZ-272 is opt-in per kind; future work may pin a JSON schema for c5.state.smoothed_history once the record shape is verified in flight tests.

Risks accepted

• Extra marginals compute per current_estimate — measured well under the 5 ms NFR in current fixtures; if a profiling pass later reveals contention we can share the marginals between the live path and the hook.
• Hook silent on partial failure — by design (the forward-time path has already returned to the caller). Forensic logs land in the structured c5.state.smoothed_history_fdr_*_failed log records.