[AZ-383] C5 add_vio/add_pose_anchor/add_fc_imu factor adds

Replaces AZ-382 NotImplementedError placeholders with real GTSAM factor
adds wired against the iSAM2 graph handle:

- add_vio  -> BetweenFactorPose3 between consecutive VIO pose keys
  (first call primes the chain; AZ-388 owns first-keyframe seeding).
- add_pose_anchor -> mode-dispatch per pose.covariance_mode:
  "marginals" -> PriorFactorPose3 + handle.update();
  "jacobian"  -> skip iSAM2 add per AZ-361 contract.
  Both paths bump _last_anchor_ns via time.monotonic_ns().
- add_fc_imu -> shared ImuPreintegrator.integrate_window +
  reset_for_new_keyframe; builds a CombinedImuFactor between the
  prev/curr (X, V, B) keyframe triple. Introduces new 'v' (velocity)
  and 'b' (bias) GTSAM key namespaces decoupled from the VIO/pose
  frame_id mapping.

Invariant 2 - non-decreasing timestamps - enforced per call with
EstimatorDegradedError + c5.state.out_of_order log. Every successful
add emits a structured DEBUG *_ok log; every failure emits a
structured ERROR *_failed log and raises through the C5 error
hierarchy (R05).

Contract-vs-reality fix-ups also landed:

- StateEstimator Protocol: add_fc_imu(ImuWindow) - was incorrectly
  annotated as ImuTelemetrySample by AZ-381.
- _last_anchor_ns semantics switched to monotonic_ns() to match
  last_anchor_age_ms.
- create() factory back-wires the ISam2GraphHandle to the estimator
  via the new attach_handle() method.

Tests: +21 in tests/unit/c5_state/test_az383_factor_adds.py covering
all 8 ACs with mock ISam2GraphHandle instances. Three obsolete
AZ-382 tests (test_ac10_add_*_raises_named_az383) removed. Full
suite: 565 passed, 2 skipped.

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

tests/unit/c5_state/test_az382_isam2_smoother_wiring.py

@@ -369,24 +369,14 @@ def test_ac9_update_emits_success_log(caplog: pytest.LogCaptureFixture) -> None:
 
 # ---------------------------------------------------------------------
 # AC-10: StateEstimator Protocol methods still raise NotImplementedError
-
-
-def test_ac10_add_vio_raises_named_az383() -> None:
-    estimator = _build_estimator()
-    with pytest.raises(NotImplementedError, match="AZ-383"):
-        estimator.add_vio(mock.MagicMock())
-
-
-def test_ac10_add_pose_anchor_raises_named_az383() -> None:
-    estimator = _build_estimator()
-    with pytest.raises(NotImplementedError, match="AZ-383"):
-        estimator.add_pose_anchor(mock.MagicMock())
-
-
-def test_ac10_add_fc_imu_raises_named_az383() -> None:
-    estimator = _build_estimator()
-    with pytest.raises(NotImplementedError, match="AZ-383"):
-        estimator.add_fc_imu(mock.MagicMock())
+#
+# Note: The three ``add_*`` methods USED to raise ``NotImplementedError``
+# naming AZ-383 in AZ-382's scope. AZ-383 has since landed and replaced
+# those bodies with real factor adds; the now-active behaviour is
+# tested in ``tests/unit/c5_state/test_az383_factor_adds.py``. Only the
+# three output methods (``current_estimate`` / ``smoothed_history`` /
+# ``health_snapshot``) still raise NotImplementedError pointing at the
+# next-task AZ-384.
 
 
 def test_ac10_current_estimate_raises_named_az384() -> None:

tests/unit/c5_state/test_az383_factor_adds.py

@@ -0,0 +1,407 @@
+"""AZ-383 — GtsamIsam2StateEstimator add_vio / add_pose_anchor / add_fc_imu.
+
+Eight ACs from ``_docs/02_tasks/done/AZ-383_c5_factor_adds.md``:
+
+- AC-1  VIO factor add — ``BetweenFactorPose3`` with correct keys +
+        noise model.
+- AC-2  Pose-anchor MARGINALS path — ``PriorFactorPose3`` + update,
+        ``_last_anchor_ns`` bumped.
+- AC-3  Pose-anchor JACOBIAN path SKIPS iSAM2 add but bumps
+        ``_last_anchor_ns`` and emits INFO log.
+- AC-4  IMU factor add via the shared ``ImuPreintegrator``.
+- AC-5  Timestamp ordering — out-of-order ``add_*`` raises
+        :class:`EstimatorDegradedError`.
+- AC-6  Defensive logging on every factor add (R05).
+- AC-7  Each ``add_*`` triggers ``handle.update()`` exactly once.
+- AC-8  ``_last_anchor_ns`` accuracy via ``last_anchor_age_ms``.
+
+The handle is replaced with a stub on every test so the asserts
+fire on call counts + factor types rather than iSAM2 convergence
+(real iSAM2 first-keyframe seeding is AZ-388's responsibility).
+"""
+
+from __future__ import annotations
+
+import logging
+from datetime import datetime, timezone
+from unittest import mock
+
+import gtsam
+import numpy as np
+import pytest
+
+from gps_denied_onboard._types.nav import ImuSample, ImuWindow
+from gps_denied_onboard._types.pose import PoseEstimate
+from gps_denied_onboard._types.vio import VioOutput
+from gps_denied_onboard.components.c5_state.config import C5StateConfig
+from gps_denied_onboard.components.c5_state.errors import EstimatorDegradedError
+from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
+    GtsamIsam2StateEstimator,
+    create,
+)
+from gps_denied_onboard.runtime_root.state_factory import clear_state_registry
+
+
+@pytest.fixture(autouse=True)
+def _registry_isolation():
+    # Arrange
+    clear_state_registry()
+    yield
+    clear_state_registry()
+
+
+def _build_estimator(*, with_stub_handle: bool = True) -> GtsamIsam2StateEstimator:
+    block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15)
+    cfg = mock.MagicMock()
+    cfg.components = {"c5_state": block}
+    estimator, _real_handle = create(
+        config=cfg,
+        imu_preintegrator=mock.MagicMock(),
+        se3_utils=mock.MagicMock(),
+        wgs_converter=mock.MagicMock(),
+        fdr_client=mock.MagicMock(),
+    )
+    if with_stub_handle:
+        estimator._isam2_handle = mock.MagicMock()
+    return estimator
+
+
+def _make_vio(*, frame_id: int, t_seconds: float, pose: np.ndarray | None = None) -> VioOutput:
+    return VioOutput(
+        frame_id=frame_id,
+        timestamp=datetime.fromtimestamp(t_seconds, tz=timezone.utc),
+        pose_se3=pose if pose is not None else np.eye(4),
+        covariance_6x6=np.eye(6) * 0.01,
+    )
+
+
+def _make_pose(
+    *,
+    frame_id: int,
+    t_seconds: float,
+    covariance_mode: str = "marginals",
+    pose: np.ndarray | None = None,
+) -> PoseEstimate:
+    return PoseEstimate(
+        frame_id=frame_id,
+        timestamp=datetime.fromtimestamp(t_seconds, tz=timezone.utc),
+        pose_se3=pose if pose is not None else np.eye(4),
+        covariance_6x6=np.eye(6) * 0.01,
+        covariance_mode=covariance_mode,
+    )
+
+
+def _make_imu_window(*, ts_start_ns: int, ts_end_ns: int) -> ImuWindow:
+    samples = (
+        ImuSample(ts_ns=ts_start_ns, accel_xyz=(0.0, 0.0, 9.81), gyro_xyz=(0.0, 0.0, 0.0)),
+        ImuSample(ts_ns=ts_end_ns, accel_xyz=(0.0, 0.0, 9.81), gyro_xyz=(0.0, 0.0, 0.0)),
+    )
+    return ImuWindow(samples=samples, ts_start_ns=ts_start_ns, ts_end_ns=ts_end_ns)
+
+
+# ---------------------------------------------------------------------
+# AC-1: VIO factor add — BetweenFactorPose3 with correct keys + noise
+
+
+def test_ac1_vio_first_call_records_without_factor() -> None:
+    estimator = _build_estimator()
+    vio = _make_vio(frame_id=1, t_seconds=1000.0)
+
+    estimator.add_vio(vio)
+
+    # First call MUST NOT emit a factor or update — there's no prev frame.
+    estimator._isam2_handle.add_factor.assert_not_called()
+    estimator._isam2_handle.update.assert_not_called()
+    assert estimator._prev_vio is vio
+
+
+def test_ac1_vio_second_call_adds_between_factor() -> None:
+    estimator = _build_estimator()
+    vio1 = _make_vio(frame_id=1, t_seconds=1000.0)
+    vio2 = _make_vio(frame_id=2, t_seconds=1001.0)
+
+    estimator.add_vio(vio1)
+    estimator.add_vio(vio2)
+
+    estimator._isam2_handle.add_factor.assert_called_once()
+    factor = estimator._isam2_handle.add_factor.call_args[0][0]
+    assert isinstance(factor, gtsam.BetweenFactorPose3)
+    # Keys should be the consecutive x-symbols
+    assert factor.keys()[0] == gtsam.symbol("x", 0)
+    assert factor.keys()[1] == gtsam.symbol("x", 1)
+
+
+# ---------------------------------------------------------------------
+# AC-2: Pose-anchor MARGINALS path
+
+
+def test_ac2_pose_anchor_marginals_adds_prior_factor_and_updates() -> None:
+    estimator = _build_estimator()
+    pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="marginals")
+
+    estimator.add_pose_anchor(pose)
+
+    estimator._isam2_handle.add_factor.assert_called_once()
+    factor = estimator._isam2_handle.add_factor.call_args[0][0]
+    assert isinstance(factor, gtsam.PriorFactorPose3)
+    estimator._isam2_handle.update.assert_called_once()
+
+
+def test_ac2_pose_anchor_marginals_bumps_last_anchor_ns() -> None:
+    estimator = _build_estimator()
+    pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="marginals")
+    before_ns = estimator._last_anchor_ns
+
+    estimator.add_pose_anchor(pose)
+
+    after_ns = estimator._last_anchor_ns
+    assert after_ns > before_ns
+    assert after_ns > 0
+
+
+# ---------------------------------------------------------------------
+# AC-3: Pose-anchor JACOBIAN path SKIPS factor add but bumps _last_anchor_ns
+
+
+def test_ac3_pose_anchor_jacobian_skips_factor_add(caplog: pytest.LogCaptureFixture) -> None:
+    estimator = _build_estimator()
+    pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian")
+
+    with caplog.at_level(logging.INFO):
+        estimator.add_pose_anchor(pose)
+
+    estimator._isam2_handle.add_factor.assert_not_called()
+    estimator._isam2_handle.update.assert_not_called()
+    skip_records = [r for r in caplog.records if r.kind == "c5.state.skip_isam2_jacobian_path"]
+    assert len(skip_records) == 1
+
+
+def test_ac3_pose_anchor_jacobian_still_bumps_last_anchor_ns() -> None:
+    estimator = _build_estimator()
+    pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian")
+    before_ns = estimator._last_anchor_ns
+
+    estimator.add_pose_anchor(pose)
+
+    assert estimator._last_anchor_ns > before_ns
+
+
+# ---------------------------------------------------------------------
+# AC-4: IMU factor add via shared ImuPreintegrator
+
+
+def test_ac4_imu_first_window_primes_chain_no_factor() -> None:
+    estimator = _build_estimator()
+    # Stub preintegrator returns a real PIM-like sentinel
+    estimator._imu_preintegrator.reset_for_new_keyframe.return_value = mock.MagicMock()
+    window = _make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000)
+
+    estimator.add_fc_imu(window)
+
+    estimator._imu_preintegrator.integrate_window.assert_called_once_with(window)
+    estimator._imu_preintegrator.reset_for_new_keyframe.assert_called_once()
+    # First window primes the chain — no factor yet
+    estimator._isam2_handle.add_factor.assert_not_called()
+    estimator._isam2_handle.update.assert_not_called()
+
+
+def test_ac4_imu_second_window_adds_combined_imu_factor() -> None:
+    estimator = _build_estimator()
+    estimator._imu_preintegrator.reset_for_new_keyframe.return_value = (
+        gtsam.PreintegratedCombinedMeasurements(
+            gtsam.PreintegrationCombinedParams.MakeSharedU(9.81),
+            gtsam.imuBias.ConstantBias(),
+        )
+    )
+    win1 = _make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000)
+    win2 = _make_imu_window(ts_start_ns=2_000_001, ts_end_ns=3_000_000)
+
+    estimator.add_fc_imu(win1)
+    estimator.add_fc_imu(win2)
+
+    assert estimator._imu_preintegrator.integrate_window.call_count == 2
+    estimator._isam2_handle.add_factor.assert_called_once()
+    factor = estimator._isam2_handle.add_factor.call_args[0][0]
+    assert isinstance(factor, gtsam.CombinedImuFactor)
+
+
+# ---------------------------------------------------------------------
+# AC-5: Timestamp ordering
+
+
+def test_ac5_vio_out_of_order_raises_degraded() -> None:
+    estimator = _build_estimator()
+    later = _make_vio(frame_id=1, t_seconds=1001.0)
+    earlier = _make_vio(frame_id=2, t_seconds=1000.0)
+
+    estimator.add_vio(later)
+    with pytest.raises(EstimatorDegradedError, match="out-of-order vio"):
+        estimator.add_vio(earlier)
+
+
+def test_ac5_pose_anchor_out_of_order_raises_degraded() -> None:
+    estimator = _build_estimator()
+    later = _make_pose(frame_id=10, t_seconds=1001.0)
+    earlier = _make_pose(frame_id=11, t_seconds=1000.0)
+
+    estimator.add_pose_anchor(later)
+    with pytest.raises(EstimatorDegradedError, match="out-of-order pose_anchor"):
+        estimator.add_pose_anchor(earlier)
+
+
+def test_ac5_imu_out_of_order_raises_degraded() -> None:
+    estimator = _build_estimator()
+    estimator._imu_preintegrator.reset_for_new_keyframe.return_value = mock.MagicMock()
+    later = _make_imu_window(ts_start_ns=2_000_000, ts_end_ns=3_000_000)
+    earlier = _make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000)
+
+    estimator.add_fc_imu(later)
+    with pytest.raises(EstimatorDegradedError, match="out-of-order imu_window"):
+        estimator.add_fc_imu(earlier)
+
+
+def test_ac5_emits_out_of_order_log(caplog: pytest.LogCaptureFixture) -> None:
+    estimator = _build_estimator()
+    later = _make_vio(frame_id=1, t_seconds=1001.0)
+    earlier = _make_vio(frame_id=2, t_seconds=1000.0)
+    estimator.add_vio(later)
+
+    with caplog.at_level(logging.ERROR):
+        with pytest.raises(EstimatorDegradedError):
+            estimator.add_vio(earlier)
+
+    err_records = [r for r in caplog.records if r.kind == "c5.state.out_of_order"]
+    assert len(err_records) == 1
+    assert err_records[0].kv["source"] == "vio"
+
+
+# ---------------------------------------------------------------------
+# AC-6: Defensive logging on every factor add (R05)
+
+
+def test_ac6_vio_success_emits_debug_log(caplog: pytest.LogCaptureFixture) -> None:
+    estimator = _build_estimator()
+    vio1 = _make_vio(frame_id=1, t_seconds=1000.0)
+    vio2 = _make_vio(frame_id=2, t_seconds=1001.0)
+
+    with caplog.at_level(logging.DEBUG):
+        estimator.add_vio(vio1)
+        estimator.add_vio(vio2)
+
+    ok_records = [r for r in caplog.records if r.kind == "c5.state.add_vio_ok"]
+    assert len(ok_records) == 1
+
+
+def test_ac6_pose_anchor_success_emits_debug_log(caplog: pytest.LogCaptureFixture) -> None:
+    estimator = _build_estimator()
+    pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="marginals")
+
+    with caplog.at_level(logging.DEBUG):
+        estimator.add_pose_anchor(pose)
+
+    ok_records = [r for r in caplog.records if r.kind == "c5.state.add_pose_anchor_ok"]
+    assert len(ok_records) == 1
+
+
+def test_ac6_vio_failure_emits_error_log_and_raises(caplog: pytest.LogCaptureFixture) -> None:
+    estimator = _build_estimator()
+    # First call records the prev_vio without going through add_factor.
+    estimator.add_vio(_make_vio(frame_id=1, t_seconds=1000.0))
+    # Make the handle fail.
+    estimator._isam2_handle.add_factor.side_effect = RuntimeError("synthetic")
+
+    with caplog.at_level(logging.ERROR):
+        with pytest.raises(EstimatorDegradedError, match="synthetic"):
+            estimator.add_vio(_make_vio(frame_id=2, t_seconds=1001.0))
+
+    err_records = [r for r in caplog.records if r.kind == "c5.state.add_vio_failed"]
+    assert len(err_records) == 1
+
+
+# ---------------------------------------------------------------------
+# AC-7: Each add_* triggers handle.update() exactly once
+
+
+def test_ac7_vio_triggers_update_once() -> None:
+    estimator = _build_estimator()
+    estimator.add_vio(_make_vio(frame_id=1, t_seconds=1000.0))
+    estimator.add_vio(_make_vio(frame_id=2, t_seconds=1001.0))
+
+    # Only the second call (which adds a factor) triggers update.
+    assert estimator._isam2_handle.update.call_count == 1
+
+
+def test_ac7_pose_anchor_marginals_triggers_update_once() -> None:
+    estimator = _build_estimator()
+    estimator.add_pose_anchor(_make_pose(frame_id=10, t_seconds=1000.0))
+
+    assert estimator._isam2_handle.update.call_count == 1
+
+
+def test_ac7_pose_anchor_jacobian_triggers_no_update() -> None:
+    estimator = _build_estimator()
+    estimator.add_pose_anchor(_make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian"))
+
+    assert estimator._isam2_handle.update.call_count == 0
+
+
+def test_ac7_imu_second_window_triggers_update_once() -> None:
+    estimator = _build_estimator()
+    estimator._imu_preintegrator.reset_for_new_keyframe.return_value = (
+        gtsam.PreintegratedCombinedMeasurements(
+            gtsam.PreintegrationCombinedParams.MakeSharedU(9.81),
+            gtsam.imuBias.ConstantBias(),
+        )
+    )
+    estimator.add_fc_imu(_make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000))
+    estimator.add_fc_imu(_make_imu_window(ts_start_ns=2_000_001, ts_end_ns=3_000_000))
+
+    assert estimator._isam2_handle.update.call_count == 1
+
+
+# ---------------------------------------------------------------------
+# AC-8: _last_anchor_ns accuracy via last_anchor_age_ms
+
+
+def test_ac8_last_anchor_age_ms_after_anchor_is_small() -> None:
+    block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15)
+    cfg = mock.MagicMock()
+    cfg.components = {"c5_state": block}
+    estimator, real_handle = create(
+        config=cfg,
+        imu_preintegrator=mock.MagicMock(),
+        se3_utils=mock.MagicMock(),
+        wgs_converter=mock.MagicMock(),
+        fdr_client=mock.MagicMock(),
+    )
+    # Use the JACOBIAN path so we exercise the _last_anchor_ns bump
+    # without needing real iSAM2 seeding (AZ-388's territory).
+    pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian")
+
+    estimator.add_pose_anchor(pose)
+    age_ms = real_handle.last_anchor_age_ms()
+
+    assert 0 <= age_ms < 1000
+
+
+# ---------------------------------------------------------------------
+# Cross-cutting: estimator requires a handle
+
+
+def test_no_handle_raises_state_estimator_config_error() -> None:
+    block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15)
+    cfg = mock.MagicMock()
+    cfg.components = {"c5_state": block}
+    # Construct directly without attach_handle.
+    estimator = GtsamIsam2StateEstimator(
+        cfg,
+        imu_preintegrator=mock.MagicMock(),
+        se3_utils=mock.MagicMock(),
+        wgs_converter=mock.MagicMock(),
+        fdr_client=mock.MagicMock(),
+    )
+
+    from gps_denied_onboard.components.c5_state.errors import StateEstimatorConfigError
+
+    with pytest.raises(StateEstimatorConfigError, match="not attached"):
+        estimator.add_vio(_make_vio(frame_id=1, t_seconds=1000.0))