gps-denied-onboard/_docs/03_implementation/batch_14_cycle1_report.md

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 ImuWindows 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.