# 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).
