# Batch 14 — Cycle 1 Implementation Report

**Batch**: 14 of N
**Tasks landed**: AZ-383 (`GtsamIsam2StateEstimator` — VIO + Pose + IMU factor adds)
**Cycle**: 1
**Date**: 2026-05-11

## Scope

| Task | Component | Purpose |
|------|-----------|---------|
| AZ-383 | C5 state estimator | Replaces the three AZ-382 `NotImplementedError` placeholders on `GtsamIsam2StateEstimator` (`add_vio`, `add_pose_anchor`, `add_fc_imu`) with real factor-add bodies wired against the AZ-382 `ISam2GraphHandle`. `add_vio` emits a `BetweenFactorPose3` between consecutive VIO pose keys (first call primes the chain only). `add_pose_anchor` mode-dispatches per `pose.covariance_mode`: `"marginals"` → `PriorFactorPose3` + `handle.update()`; `"jacobian"` → SKIP the iSAM2 add per the AZ-361 cross-task contract (the running estimate still consumes the pose downstream, the graph stops growing under throttle). Both paths bump `_last_anchor_ns` via `time.monotonic_ns()` so `last_anchor_age_ms` and the spoof-promotion gate (AZ-385) see a recent satellite anchor. `add_fc_imu` feeds the shared `ImuPreintegrator`, closes the PIM with `reset_for_new_keyframe()`, and builds a `CombinedImuFactor` between the previous and current `(X, V, B)` keyframe triple — first call primes the chain without emitting a factor. Every successful add emits a DEBUG `*_ok` log; every failure emits an ERROR `*_failed` log and raises through the C5 error hierarchy (R05). Invariant 2 — non-decreasing timestamps — is enforced per call with an `EstimatorDegradedError` + `c5.state.out_of_order` log on violation. The remaining three Protocol output methods (`current_estimate` / `smoothed_history` / `health_snapshot`) still raise `NotImplementedError` naming AZ-384. |

## Decision record

The task started as a "~5pt drop-in", but inspecting the contracts surfaced four contract-vs-reality conflicts that the user chose to resolve in full alignment with the C5 contract rather than paper over (re-estimate to ~7–8pt, one batch):

1. **Protocol `add_fc_imu` type mismatch** — AZ-381's Protocol annotated `add_fc_imu(ImuTelemetrySample)` (a single sample), but the C5 contract has always specified `ImuWindow` because the shared `ImuPreintegrator` operates on batches. The composition root will batch the C8 `ImuTelemetrySample` stream into `ImuWindow`s before calling `add_fc_imu`. Fixed `interface.py` here with a docstring note.
2. **`ImuPreintegrator` API mismatch** — task spec assumed `preintegrate(window) -> CombinedImuFactor`. Real API per AZ-276 is `integrate_window(window)` + `reset_for_new_keyframe() -> PreintegratedCombinedMeasurements`. C5 builds the `CombinedImuFactor` itself from the closed PIM + the (X, V, B) key triple — that's not the preintegrator's job.
3. **Anchor freshness semantics** — task spec said `_last_anchor_ns = pose.emitted_at`, but `PoseEstimate` has no `emitted_at` field and the contract describes the value as a **monotonic age** (consumed by `last_anchor_age_ms` which uses `time.monotonic_ns()`). Switched to `_last_anchor_ns = time.monotonic_ns()`.
4. **`CombinedImuFactor` key management** — the factor needs a `(prev_X, prev_V, prev_B, curr_X, curr_V, curr_B)` 6-tuple of keys. Introduced two new GTSAM key namespaces (`'v'` for velocity, `'b'` for bias) with an independent `_imu_keyframe_counter`. The `'x'` (pose) namespace is shared with VIO / pose-anchor via `_next_key_counter`, but the IMU chain steps it independently per IMU keyframe — the composition root will arrange the higher-level scheduling that fuses the chains.

## Files added / modified

### Modified (prod)

- `src/gps_denied_onboard/components/c5_state/interface.py` — corrected `add_fc_imu` Protocol annotation from `ImuTelemetrySample` to `ImuWindow`; added a docstring note explaining the AZ-381 → AZ-383 fix-up.
- `src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py` — replaced the three `NotImplementedError` placeholders with real factor-add bodies; added `_DEFAULT_POSE_SIGMA = 0.1` module constant; added new state fields: `_log`, `_isam2_handle` (back-reference to the handle), `_last_added_ts_ns` (Invariant 2 monotonic guard), `_prev_vio` (between-factor wiring), `_imu_keyframe_counter` + `_prev_imu_x_key/_prev_imu_v_key/_prev_imu_b_key` (IMU chain); added `attach_handle(handle)` method; extended `key_for_frame` signature to `UUID | int`; added internal helpers `_require_handle`, `_guard_timestamp`, `_reset_staging`; added module-level pure helpers `_datetime_to_ns`, `_pose_se3_to_gtsam`, `_build_pose_noise`, `_make_timestamp_map`; updated `create(...)` to back-wire the handle to the estimator via `attach_handle(...)`.

### Added (tests)

- `tests/unit/c5_state/test_az383_factor_adds.py` — 21 new tests covering all 8 ACs using mock `ISam2GraphHandle` instances (real iSAM2 would require seeded initial values which AZ-388 owns — see "Test strategy" below).

### Modified (tests)

- `tests/unit/c5_state/test_az382_isam2_smoother_wiring.py` — removed the three now-obsolete `test_ac10_add_*_raises_named_az383` tests; their behaviour is now governed by AZ-383's AC tests. Replaced with a comment block pointing readers to `test_az383_factor_adds.py`. The three remaining `test_ac10_*_raises_named_az384` tests (the marginals/outputs methods) were retained.

## Test strategy — why mock handles

A smoke test of `add_vio` against the real `ISam2GraphHandleImpl` failed with `iSAM2.update failed: Attempting to … the key "x0", which does not exist in the Values`. The reason: a between-factor between `x0` and `x1` requires both keys to already be in `gtsam.Values`. The first `add_vio` call would need to ALSO seed `x0` with a prior or an initial value, which is the responsibility of AZ-388 (AC-5.2 fallback). Rather than smuggle that responsibility into AZ-383, the AC tests verify only what AZ-383 owns:

- The correct GTSAM factor TYPE is constructed (`BetweenFactorPose3` / `PriorFactorPose3` / `CombinedImuFactor`).
- The correct keys are passed.
- `handle.add_factor` and `handle.update` are called the expected number of times.
- The mode-dispatch in `add_pose_anchor` correctly SKIPS the add on the JACOBIAN path.
- Invariant 2 enforcement raises `EstimatorDegradedError` + emits the structured `c5.state.out_of_order` log.
- Every success path emits a `*_ok` DEBUG log; every failure path emits a `*_failed` ERROR log and raises through the C5 error hierarchy.
- `_last_anchor_ns` is updated on BOTH pose-anchor paths.

The full end-to-end iSAM2 convergence will be tested in AZ-388 once the chain is seedable.

## Architectural notes

- **`_FRAME_PERIOD_S` constant** — inherited from AZ-382 (D-C5-3 fixes the keyframe rate at 3 Hz). AZ-383 did not touch the smoother window logic.
- **(X, V, B) chain is independent of the frame-id mapping** — IMU keyframes are NOT VIO keyframes; the contract documents this. The composition root will fuse the chains; AZ-383's per-window factor add is self-contained.
- **Defensive logging is mandatory (R05)** — every successful add: `c5.state.add_vio_ok` / `c5.state.add_pose_anchor_ok` / `c5.state.add_fc_imu_ok`. Every failure: `c5.state.add_vio_failed` / `c5.state.add_pose_anchor_failed` / `c5.state.add_fc_imu_failed` / `c5.state.add_fc_imu_preintegrate_failed`. JACOBIAN-path skip emits INFO `c5.state.skip_isam2_jacobian_path`. Out-of-order: ERROR `c5.state.out_of_order`. The C5 contract documents this surface because silent factor-add failures bit the prototype.
- **First-call priming** — both `add_vio` and `add_fc_imu` SHORT-CIRCUIT the first call (`_prev_vio is None` / `_prev_imu_x_key is None`) because there is no previous frame to relate to. This is documented in both method docstrings.
- **`_reset_staging()` after every successful flush** — the staging `_graph` / `_values` are cleared post-flush so the next `add_*` builds a fresh delta. This matches the iSAM2 incremental-update idiom (the handle's `update(graph, values, …)` accepts the DELTA, not the cumulative graph).

## Test counts

| Suite | Before (B13) | After (B14) | Delta |
|-------|--------------|-------------|-------|
| Total passing | 547 | 565 | +18 |
| Skipped | 2 | 2 | 0 |
| AZ-383 (new) | 0 | 21 | +21 |
| AZ-382 (preserved) | 27 | 24 | −3 (obsolete `test_ac10_add_*_raises_named_az383` tests removed; behaviour now lives in AZ-383 tests) |

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

## Lint / type

- `ruff check src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/` — clean (1 `RUF002` ambiguous-Unicode in a docstring auto-fixed during the cycle).
- `ruff format src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/` — clean (1 file reformatted).
- `ReadLints` on `src/gps_denied_onboard/components/c5_state/` + `tests/unit/c5_state/` — 0 errors.

## Acceptance evidence

| AC | Test(s) | Status |
|----|---------|--------|
| AC-1 VIO factor add | `test_ac1_vio_first_call_records_without_factor`, `test_ac1_vio_second_call_adds_between_factor` | PASS |
| AC-2 Pose-anchor MARGINALS path | `test_ac2_pose_anchor_marginals_adds_prior_factor_and_updates`, `test_ac2_pose_anchor_marginals_bumps_last_anchor_ns` | PASS |
| AC-3 Pose-anchor JACOBIAN skip | `test_ac3_pose_anchor_jacobian_skips_factor_add`, `test_ac3_pose_anchor_jacobian_still_bumps_last_anchor_ns` | PASS |
| AC-4 IMU factor add via ImuPreintegrator | `test_ac4_imu_first_window_primes_chain_no_factor`, `test_ac4_imu_second_window_adds_combined_imu_factor` | PASS |
| AC-5 Timestamp ordering | `test_ac5_vio_out_of_order_raises_degraded`, `test_ac5_pose_anchor_out_of_order_raises_degraded`, `test_ac5_imu_out_of_order_raises_degraded`, `test_ac5_emits_out_of_order_log` | PASS |
| AC-6 Defensive logging | `test_ac6_vio_success_emits_debug_log`, `test_ac6_pose_anchor_success_emits_debug_log`, `test_ac6_vio_failure_emits_error_log_and_raises` | PASS |
| AC-7 Update exactly once | `test_ac7_vio_triggers_update_once`, `test_ac7_pose_anchor_marginals_triggers_update_once`, `test_ac7_pose_anchor_jacobian_triggers_no_update`, `test_ac7_imu_second_window_triggers_update_once` | PASS |
| AC-8 `_last_anchor_ns` accuracy | `test_ac8_last_anchor_age_ms_after_anchor_is_small` | PASS |
| Defensive — no-handle | `test_no_handle_raises_state_estimator_config_error` | PASS |

## Known forward actions (not in scope this batch)

- **AZ-388 (AC-5.2 fallback)** — first-keyframe seeding (prior factor + initial values on x0) so an end-to-end smoke test of `add_vio` → real iSAM2 update converges. The current AZ-383 tests use mock handles to avoid coupling the factor-add semantics to that scheduling.
- **AZ-384** — `current_estimate` / `smoothed_history` / `health_snapshot` bodies; will read marginals from the handle and translate them into the C5 `EstimatorOutput` / `EstimatorHealth` DTOs.
- **Composition root batching** — turn the C8 `ImuTelemetrySample` stream into `ImuWindow` instances before calling `add_fc_imu`. Owned by the composition-root binding task.
- **Cross-stream timestamp merge** — AZ-383 enforces per-call monotonicity but does not order the three streams against each other; the composition root will arrange that.