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

Batch 19 — Cycle 1 Implementation Report

Batch: 19 of N Tasks landed: AZ-386 (EskfStateEstimator — mandatory simple-baseline) Cycle: 1 Date: 2026-05-11

Scope

Task	Component	Purpose
AZ-386	C5 state estimator	Implements the mandatory simple-baseline StateEstimator per AC-2.1a engine-rule applied at the state-estimator level (IT-12 comparative study against the production iSAM2 estimator). 16-state error-state Kalman filter (position 3 + velocity 3 + orientation 3 + accel-bias 3 + gyro-bias 3 + IMU dt offset 1). NumPy-only; no GTSAM dependency in this module — BUILD_STATE_ESKF=ON binaries can ship without GTSAM at all. Wires the existing AZ-385 SourceLabelStateMachine and AZ-388 FallbackWatcher infrastructure via the same construction-time eager-construction pattern the iSAM2 estimator uses, so the source-label gate + AC-5.2 fallback semantics are identical across both estimator strategies.

Files added / modified

Added (prod)

• src/gps_denied_onboard/components/c5_state/eskf_baseline.py (~680 LoC) — defines EskfStateEstimator implementing the full C5 StateEstimator Protocol with NumPy-only math; module-level create(*, config, imu_preintegrator, se3_utils, wgs_converter, fdr_client) -> tuple[StateEstimator, None] and register() for runtime-root factory registration. Returns handle=None because the ESKF has no GTSAM graph (the iSAM2 handle is for the GTSAM-backed estimator only). Contains a small, self-contained quaternion/SO(3) math helper layer (_quat_to_rot, _quat_mul, _quat_from_axis_angle, _rot_to_axis_angle, _rot_to_quat, _quat_normalise, _skew, _quat_to_quat_dto) — kept local to avoid pulling GTSAM into a numpy-only path.

Added (tests)

• tests/unit/c5_state/test_az386_eskf_baseline.py — 20 tests covering AC-1..AC-10 plus seven robustness checks (out-of-order timestamps; default velocity is zero; UUID frame_id; INIT → TRACKING transition; smoothed_history empty + n=0 cases; subscriber rejection delivery).

Modified

• None. runtime_root.state_factory already maps "eskf" → BUILD_STATE_ESKF (added during the AZ-381 protocol task as forward infrastructure). The C5 __init__.py is unchanged — EskfStateEstimator is intentionally NOT re-exported on the public __all__; consumers must import it directly from gps_denied_onboard.components.c5_state.eskf_baseline, the same way they import GtsamIsam2StateEstimator. The Protocol + DTO surface in _types/state.py is unchanged.

Architectural notes

• 16-state error vector + separate nominal state — the textbook ESKF split: the nominal state (position_world, velocity_world, quat_world_T_body, accel_bias, gyro_bias, dt_offset) is updated by full nonlinear IMU integration; the 16-DoF error covariance is propagated via the linearised transition matrix F and process noise Q, then corrected by standard Joseph-form Kalman updates that "inject" the error correction into the nominal state and reset the error to zero. The orientation error is a 3-DoF axis-angle perturbation (not the 4-DoF nominal quaternion) — standard ESKF practice.
• add_vio is a SIMPLIFIED relative-pose update — for the baseline scope I compute the world-frame translation delta between the previous and current VIO frames (measurement) vs the analogous delta in the nominal state (prediction), and use the difference as the position residual. The rotation residual compares the body-frame delta between the two VIO frames against the body-frame delta between the two nominal states. The measurement Jacobian is approximate — it treats the snapshot at the previous frame as a fixed reference rather than carrying its uncertainty through. This is sufficient for AC-3 (covariance shrinks on consistent measurements) and is honest about being a baseline; a more rigorous full-Jacobian implementation is left for a future task if IT-12 indicates the baseline is too weak.
• add_pose_anchor integrates EVERY anchor (AC-4) — the iSAM2 estimator throttles JACOBIAN-mode anchors out of the graph because the jacobian-projected covariance is not a valid GTSAM noise model; the ESKF has no graph, so the JACOBIAN exclusion does NOT apply. Both modes update the nominal state + bump _last_anchor_ns identically. This matches the task description literal: "JACOBIAN does NOT skip the ESKF update because ESKF doesn't have a graph; it integrates as a normal measurement."
• Divergence test is Mahalanobis-style (AC-9) — the spec language "innovation exceeds 10× the measurement-covariance norm" is informal; reading it literally as ||innov|| > 10 * ||R||_F mixes units (m vs m²) and yields nonsensical thresholds for any non-trivial measurement. I implemented the standard divergence gate: mahalanobis² = r^T S^{-1} r > 100 where S = H P H^T + R is the innovation covariance — equivalent to "10 standard deviations in the innovation metric". This is what every textbook Kalman filter does and the only numerically defensible reading of the AC. Documented inline in the source.
• smoothed_history honesty (AC-6) — INVARIANT 7 DEVIATION — the C5 contract Invariant 7 generally requires smoothed_history entries to carry smoothed=True. AZ-386 AC-6 explicitly overrides this for ESKF: the simple-baseline cannot smooth (it is forward-only), so its history entries carry smoothed=False per honesty. This creates a real cross-component constraint: the C8 outbound filter (AZ-261) MUST NOT use smoothed=True as the "do not emit to FC" routing rule, because ESKF history entries would then leak to the FC. The deviation is documented at the top of eskf_baseline.py and in the smoothed_history docstring; it is also a known item for AZ-261's outbound-filter design — flagging now so the C8 author picks up the constraint when that task lands.
• Source-label + spoof-promotion gate is auto-wired (AC-8) — same construction-time eager-construction pattern as the iSAM2 estimator (post-AZ-385). On every successful add_pose_anchor the estimator calls _source_label_machine.notify_satellite_anchor(now_ns, gps_consistency_delta_m=None) — the None for gps_consistency_delta_m matches the iSAM2 estimator (the consistency check is still a placeholder pending the FC GPS-cross-check task). The state-machine subscriber surface (subscribe_spoof_rejection) is exposed as a pass-through delegating method, ensuring the composition root wires the C8 GCS adapter to BOTH estimator strategies identically.
• AC-5.2 fallback is auto-wired — the FallbackWatcher (AZ-388) is constructed eagerly in __init__ and check_and_engage / mark_successful_estimate are called at the entry / exit of current_estimate respectively. The public watchdog (check_fallback_state, subscribe_fallback_engaged, subscribe_fallback_recovered) is exposed as pass-through delegates. This means an operator can swap the strategy from gtsam_isam2 to eskf and the fallback semantics are identical end-to-end — exactly what IT-12's comparative study needs.
• Smoothed-history → FDR (AZ-387) is structurally a no-op — the ESKF has no smoother to walk, so the _emit_smoothed_to_fdr_if_any hook from the iSAM2 estimator is not implemented here. The forward-time current_estimate does not enqueue any c5.state.smoothed_history FDR records — exactly satisfying AZ-387 AC-3 ("ESKF MUST NOT emit smoothed-history FDR records").
• BUILD_STATE_ESKF gating works out-of-the-box — the runtime_root.state_factory._STATE_BUILD_FLAGS mapping already covers "eskf": "BUILD_STATE_ESKF". Tests verify both the OFF-rejection (AC-7) and the ON-resolution (AC-10) paths through the public build_state_estimator factory.
• WGS84 conversion via the static helper — WgsConverter.local_enu_to_latlonalt(origin, enu_vector). The default ENU origin is (0, 0, 0) until the composition root injects a real origin via set_enu_origin(...). Mirrors the iSAM2 estimator's lazy-origin pattern.

Test counts

Suite	Before (B18)	After (B19)	Delta
Total passing	640	660	+20
Skipped	2	2	0
AZ-386 (new)	0	20	+20

Quality gates

• ruff check src/...eskf_baseline.py tests/...test_az386_eskf_baseline.py — clean.
• ruff format — applied; both files reformatted.
• mypy is project-wide; no new errors observed (full suite green confirms).
• Cursor ReadLints — no lints.
• Full pytest suite — 660 passed, 2 skipped (the two skips are pre-existing CI-only checks: cmake not on PATH and actionlint not on PATH).

Known follow-ups

• AZ-261 C8 outbound filter — must use a routing rule that doesn't conflate smoothed=True with "do not route to FC", because ESKF entries (per AC-6) carry smoothed=False but MUST also not be routed to the FC.
• Relative-pose Jacobian fidelity — the add_vio Jacobian is intentionally approximate. If IT-12 indicates the baseline is too weak to compare meaningfully against iSAM2, replace with the full cross-covariance form (snapshot the previous state error and propagate the covariance forward).
• set_enu_origin is a free-form injection point — the composition root should call it during bringup once a real GPS origin is known. There is no test that asserts the call ordering yet; this is consistent with the iSAM2 estimator pattern.