[AZ-387] C5 smoothed-history → FDR side-channel

After every successful current_estimate(), emit one
c5.state.smoothed_history FDR record per newly-smoothed past
keyframe from IncrementalFixedLagSmoother. AC-4.5 (revised): the
smoothed stream goes ONLY to FDR; the C8 outbound forward-time
stream is unaffected.

Idempotency via _smoothed_fdr_watermark_s (smoother-native float
seconds); the same pose key is never emitted twice. Hook is
best-effort — internal failures log warnings but do not raise, so
a smoother divergence cannot contaminate the forward-time path.

Cross-task invariants documented:
- AC-3 ESKF no-op — AZ-386 installs an inert hook on the ESKF.
- AC-4 No C8 leak — enforced at the C8 boundary by AZ-261.

8 new unit tests against AC-1/2/5/6 + robustness (no-FDR-client,
marginals failure). Full suite: 640 passed, 2 skipped.

Co-authored-by: Cursor <cursoragent@cursor.com>

src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py

@@ -32,6 +32,7 @@ from __future__ import annotations
 
 import time
 from collections import deque
+from datetime import datetime, timezone
 from typing import TYPE_CHECKING, Any, Final
 from uuid import UUID, uuid4
 
@@ -70,6 +71,7 @@ from gps_denied_onboard.components.c5_state.errors import (
     StateEstimatorConfigError,
 )
 from gps_denied_onboard.components.c5_state.interface import StateEstimator
+from gps_denied_onboard.fdr_client.records import FdrRecord
 from gps_denied_onboard.helpers.wgs_converter import WgsConverter
 from gps_denied_onboard.logging import get_logger
 
@@ -223,6 +225,15 @@ class GtsamIsam2StateEstimator(StateEstimator):
         # ``LOST`` on a fatal SPD failure or GTSAM exception.
         self._isam2_state: IsamState = IsamState.INIT
 
+        # AZ-387 state -----------------------------------------------------
+        # Watermark for the smoothed-history → FDR hook. We only emit a
+        # smoothed past-keyframe to FDR if its smoother timestamp is
+        # strictly greater than this watermark; the watermark is bumped
+        # to the largest emitted timestamp after every call. Idempotent
+        # by construction (AC-5 — same key never emitted twice). The
+        # watermark is in seconds (float, smoother native units).
+        self._smoothed_fdr_watermark_s: float = float("-inf")
+
         # AZ-388 state -----------------------------------------------------
         # AC-5.2 fallback watcher — engages when ``current_estimate``
         # cannot produce a fresh output for ``no_estimate_fallback_s``
@@ -729,6 +740,16 @@ class GtsamIsam2StateEstimator(StateEstimator):
         # fires the recovery signal if we were previously engaged.
         self._fallback.mark_successful_estimate(emitted_at)
 
+        # AZ-387: smoothed-history → FDR side-channel. Emits any
+        # newly-smoothed past-keyframes to FDR with ``smoothed=True``;
+        # idempotent via the timestamp watermark. AC-4.5 forbids
+        # routing smoothed estimates to C8 outbound — that filter is
+        # enforced on the C8 side and documented as a cross-task
+        # invariant. The hook is best-effort: a failure here is
+        # logged but does NOT raise (the forward-time estimate has
+        # already been computed).
+        self._emit_smoothed_to_fdr_if_any(emitted_at, source_label, last_anchor_age_ms)
+
         return EstimatorOutput(
             frame_id=uuid4(),
             position_wgs84=position_wgs84,
@@ -834,6 +855,124 @@ class GtsamIsam2StateEstimator(StateEstimator):
             )
         return out
 
+    def _emit_smoothed_to_fdr_if_any(
+        self,
+        emitted_at_ns: int,
+        source_label: PoseSourceLabel,
+        last_anchor_age_ms: int,
+    ) -> None:
+        """AZ-387 hook — emit newly-smoothed past-keyframes to FDR.
+
+        AC-4.5 (revised): smoothed past-keyframes are an onboard-only
+        side-channel for forensics. They MUST land in FDR (so post-
+        flight tooling can refine the trajectory with later evidence)
+        and they MUST NOT reach C8 outbound (the FC expects a
+        forward-time stream). This hook walks the
+        ``IncrementalFixedLagSmoother`` active POSE keys, filters to
+        those whose smoother timestamp is strictly newer than the
+        watermark, and enqueues one FDR record per key.
+
+        Idempotency (AC-5): the watermark is the largest smoother
+        timestamp ever emitted; the second call against the same
+        smoother state emits zero records. Failure path: a marginals
+        compute error logs ``c5.state.smoothed_history_marginals_failed``
+        but does NOT raise (the forward-time estimate succeeded and
+        was emitted to the caller already).
+        """
+        if self._fdr_client is None:
+            return
+        try:
+            active_values = self._smoother.calculateEstimate()
+            ts_map = self._smoother.timestamps()
+        except Exception:
+            return
+
+        new_entries: list[tuple[int, float]] = []
+        for key in active_values.keys():
+            ikey = int(key)
+            if gtsam.symbolChr(ikey) != ord("x"):
+                continue
+            try:
+                ts_sec = float(ts_map.at(ikey))
+            except Exception:
+                continue
+            if ts_sec <= self._smoothed_fdr_watermark_s:
+                continue
+            new_entries.append((ikey, ts_sec))
+        if not new_entries:
+            return
+
+        new_entries.sort(key=lambda kt: kt[1])
+
+        try:
+            handle = self._require_handle()
+            marginals = handle.compute_marginals()
+        except Exception as exc:
+            self._log.warning(
+                "c5.state.smoothed_history_fdr_marginals_failed",
+                extra={
+                    "kind": "c5.state.smoothed_history_fdr_marginals_failed",
+                    "kv": {"error": str(exc)},
+                },
+            )
+            return
+
+        for key, ts_sec in new_entries:
+            try:
+                cov = np.asarray(marginals.marginalCovariance(key), dtype=np.float64)
+                _enforce_spd(cov)
+                pose = active_values.atPose3(key)
+            except Exception as exc:
+                self._log.warning(
+                    "c5.state.smoothed_history_fdr_per_key_failed",
+                    extra={
+                        "kind": "c5.state.smoothed_history_fdr_per_key_failed",
+                        "kv": {"pose_key": key, "error": str(exc)},
+                    },
+                )
+                continue
+            position_wgs84 = self._enu_pose_to_wgs84(pose)
+            orientation = _quat_from_pose3(pose)
+            record = FdrRecord(
+                schema_version=1,
+                ts=datetime.now(tz=timezone.utc).isoformat(),
+                producer_id="c5_state",
+                kind="c5.state.smoothed_history",
+                payload={
+                    "pose_key": key,
+                    "smoother_ts_s": ts_sec,
+                    "emitted_at": emitted_at_ns,
+                    "position_wgs84": {
+                        "lat_deg": position_wgs84.lat_deg,
+                        "lon_deg": position_wgs84.lon_deg,
+                        "alt_m": position_wgs84.alt_m,
+                    },
+                    "orientation_world_T_body": {
+                        "w": orientation.w,
+                        "x": orientation.x,
+                        "y": orientation.y,
+                        "z": orientation.z,
+                    },
+                    "covariance_6x6": cov.tolist(),
+                    "source_label": source_label.value,
+                    "last_satellite_anchor_age_ms": last_anchor_age_ms,
+                    "smoothed": True,
+                },
+            )
+            try:
+                self._fdr_client.enqueue(record)
+            except Exception as exc:
+                self._log.warning(
+                    "c5.state.smoothed_history_fdr_enqueue_failed",
+                    extra={
+                        "kind": "c5.state.smoothed_history_fdr_enqueue_failed",
+                        "kv": {"pose_key": key, "error": repr(exc)},
+                    },
+                )
+                continue
+            if ts_sec > self._smoothed_fdr_watermark_s:
+                self._smoothed_fdr_watermark_s = ts_sec
+
     def health_snapshot(self) -> EstimatorHealth:
         """Return the current iSAM2 health snapshot.