[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>

_docs/02_tasks/done/AZ-383_c5_factor_adds.md

@@ -0,0 +1,87 @@
+# C5 GtsamIsam2StateEstimator — VIO + Pose + IMU factor adds
+
+**Task**: AZ-383_c5_factor_adds
+**Name**: C5 `GtsamIsam2StateEstimator` — `add_vio` / `add_pose_anchor` / `add_fc_imu` factor add bodies
+**Description**: Implement the three `add_*` factor add bodies on `GtsamIsam2StateEstimator`. `add_vio(VioOutput)` → `BetweenFactorPose3` between consecutive pose keys with VIO-derived noise model. `add_pose_anchor(PoseEstimate)` → if `pose.covariance_mode == MARGINALS`: `GenericProjectionFactorCal3DS2` (or equivalent prior factor) on the pose key; if `JACOBIAN`: skip iSAM2 add (per AZ-361 contract — the running estimate is updated but the graph stops growing under throttle). `add_fc_imu(ImuWindow)` → `CombinedImuFactor` using the shared `ImuPreintegrator` (AZ-276) for the preintegrated measurement. Out-of-order timestamps → `EstimatorDegradedError`. Each factor add is wrapped in success/false logging (R05). Updates `_last_anchor_ns` when a satellite-anchored pose is added (consumed by `last_anchor_age_ms`).
+**Complexity**: 5 points
+**Dependencies**: AZ-382 (graph + handle), AZ-381 (DTOs + handle), AZ-276 (`ImuPreintegrator`), AZ-358 (`PoseEstimate.covariance_mode` semantics), AZ-263, AZ-269, AZ-266, AZ-272
+**Component**: c5_state (epic AZ-260 / E-C5)
+**Tracker**: AZ-383
+**Epic**: AZ-260 (E-C5)
+
+### Document Dependencies
+
+- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md` — Invariants 2 (timestamp ordering), 3 (mode dispatch).
+- `_docs/02_document/components/07_c5_state/description.md` — § 5 error handling.
+- `_docs/02_document/contracts/shared_helpers/imu_preintegrator.md` — `preintegrate(window) -> CombinedImuFactor`.
+- `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md` — `covariance_mode` semantics.
+
+## Problem
+
+Without this task, the iSAM2 graph stays empty; no factors are added; `current_estimate()` (next task) cannot recover any meaningful state.
+
+## Outcome
+
+- `add_vio(vio: VioOutput)`: build `BetweenFactorPose3(prev_pose_key, curr_pose_key, vio.relative_pose, noise_model)` from `vio.covariance_relative_pose`; append via `_isam2_handle.add_factor(...)`; log SUCCESS or FAILURE.
+- `add_pose_anchor(pose: PoseEstimate)`:
+  - If `pose.covariance_mode == MARGINALS`: build `PriorFactorPose3(pose_key, pose.position+orientation, noise_model_from_cov)`; `add_factor` + `update`. Set `_last_anchor_ns = pose.emitted_at`.
+  - If `pose.covariance_mode == JACOBIAN`: skip iSAM2 factor add; INFO log `kind="c5.state.skip_isam2_jacobian_path"`. Set `_last_anchor_ns = pose.emitted_at` (still counts as a recent anchor for AC-1.3 binning).
+- `add_fc_imu(imu_window: ImuWindow)`: `cif = imu_preintegrator.preintegrate(imu_window) -> CombinedImuFactor`; `add_factor(cif)`.
+- Timestamp ordering: store `_last_added_timestamp_ns`; reject any `add_*` call whose timestamp is < `_last_added_timestamp_ns` with `EstimatorDegradedError`.
+- After every successful `add_*`, trigger `_isam2_handle.update()` per the contract.
+
+## Scope
+
+### Included
+- All three `add_*` method bodies.
+- Mode-dispatch in `add_pose_anchor` per AZ-361 cross-task contract.
+- Timestamp-ordering enforcement (Invariant 2).
+- Defensive logging on every factor add (R05).
+- Unit tests: VIO factor adds against a stub graph; pose-anchor MARGINALS path adds factor + updates `_last_anchor_ns`; pose-anchor JACOBIAN path SKIPS factor add but updates `_last_anchor_ns`; IMU factor adds via `ImuPreintegrator`; out-of-order timestamps raise `EstimatorDegradedError`; success/failure logging fires.
+
+### Excluded
+- `current_estimate` / `smoothed_history` / `health_snapshot` — owned by next task (78).
+- Source-label / spoof gate — owned by 79.
+- AC-5.2 fallback — owned by 81.
+
+## Acceptance Criteria
+
+**AC-1: VIO factor add** — Stub `_isam2_handle.add_factor` records calls; AC asserts a `BetweenFactorPose3` is added with the correct keys and noise model.
+
+**AC-2: Pose-anchor MARGINALS path** — `add_pose_anchor(pose)` with `covariance_mode = MARGINALS` adds a `PriorFactorPose3`; `_isam2_handle.update()` is called; `_last_anchor_ns = pose.emitted_at`.
+
+**AC-3: Pose-anchor JACOBIAN path skips factor add** — no `add_factor`/`update` call; INFO log emitted; `_last_anchor_ns` STILL updated.
+
+**AC-4: IMU factor add via `ImuPreintegrator`** — `imu_preintegrator.preintegrate.assert_called_once_with(imu_window)`; the returned `CombinedImuFactor` is added.
+
+**AC-5: Timestamp ordering** — out-of-order `add_*` raises `EstimatorDegradedError`; ERROR log; FDR record.
+
+**AC-6: Defensive logging on every factor add (R05)** — SUCCESS log on every successful add; FAILURE log + `EstimatorDegradedError` on iSAM2 failure.
+
+**AC-7: `add_*` triggers `update()` exactly once per call** — verified via stub.
+
+**AC-8: `_last_anchor_ns` accuracy** — after a satellite-anchored pose at t0, `last_anchor_age_ms()` returns `(t1 - t0) / 1e6` at later time t1.
+
+## Non-Functional Requirements
+
+- `add_vio` p99 ≤ 5 ms (factor build + add).
+- `add_pose_anchor` p99 ≤ 30 ms (incl. iSAM2 update).
+- `add_fc_imu` p99 ≤ 10 ms (incl. preintegration via `ImuPreintegrator`).
+
+## Constraints
+
+- Single-writer thread (Invariant 1).
+- Mode-dispatch in `add_pose_anchor` is mandatory (cross-task contract with AZ-361).
+- Timestamp ordering is enforced; out-of-order is `EstimatorDegradedError`.
+- Defensive logging is mandatory (R05).
+
+## Risks & Mitigation
+
+- **Risk: `gtsam.PriorFactorPose3` API changed in pinned version** — verify against pinned GTSAM during impl.
+- **Risk: Mode dispatch oversight** — explicit AC (AC-3) verifies the JACOBIAN path skips the iSAM2 add.
+
+## Runtime Completeness
+
+- **Named capability**: VIO / Pose / IMU factor add path.
+- **Production code**: real GTSAM factor types, real ImuPreintegrator preintegrate, real handle update calls.
+- **Unacceptable substitutes**: stub factor types in production; skipping the JACOBIAN-path skip (would corrupt the graph under throttle).