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