gps-denied-onboard/_docs/02_tasks/done/AZ-276_imu_preintegrator.md

ImuPreintegrator Helper Module

Task: AZ-276_imu_preintegrator Name: ImuPreintegrator Helper Description: Implement the shared ImuPreintegrator helper that wraps GTSAM's PreintegrationCombinedParams + PreintegratedCombinedMeasurements so C1 (VIO) and C5 (StateEstimator) consume one canonical preintegration of every FC IMU window. Single-threaded by design; one instance per writer thread, bound by the composition root. Bias drift remains the consumer's responsibility. Complexity: 2 points Dependencies: AZ-263_initial_structure Component: shared.helpers.imu_preintegrator (cross-cutting; epic AZ-264 / E-CC-HELPERS) Tracker: AZ-276 Epic: AZ-264 (E-CC-HELPERS)

Document Dependencies

• _docs/02_document/contracts/shared_helpers/imu_preintegrator.md — frozen public interface this task produces.
• _docs/02_document/common-helpers/01_helper_imu_preintegrator.md — design rationale and consumer mapping.

Problem

C1's VIO loop and C5's state estimator both consume the same FC IMU window every keyframe. Without a shared preintegrator:

• They drift into two slightly-different integrations of the same physical IMU stream (different sample-rejection rules, different bias-application order).
• The GTSAM CombinedImuFactor shape that goes into C5's iSAM2 graph diverges from the one C1 uses for its own pose update, breaking the "single source of IMU truth" invariant in solution.md.
• Per-deployment IMU noise covariances (which live in CameraCalibration) get parsed twice, with subtle unit differences.

Outcome

• A single ImuPreintegrator is the only path through which any onboard process integrates IMU samples for a GTSAM CombinedImuFactor. C1 and C5 import it; nothing else does.
• The composition root binds ONE instance per writer thread; the helper's contract test confirms it does not acquire any locks (so no surprise serialisation under load).
• Sample monotonicity is enforced — non-monotonic samples raise ImuPreintegrationError before any state is mutated.
• Re-bias is explicit: reset_with_bias is called by consumers when their bias estimate changes; the helper never re-estimates bias internally.

Scope

Included

• ImuPreintegrator class + factory make_imu_preintegrator(calibration: CameraCalibration) -> ImuPreintegrator.
• Integration entrypoints: integrate_sample(ImuSample), integrate_window(ImuWindow).
• Factor accessors: current_preintegration() -> CombinedImuFactor, reset_for_new_keyframe() -> CombinedImuFactor (destructive).
• Bias control: reset_with_bias(ImuBias) -> None.
• ImuPreintegrationError exception type re-exported alongside the helper.
• Re-export of GTSAM's CombinedImuFactor (or a thin alias) so consumers do not import GTSAM directly.
• Public interface contract published at _docs/02_document/contracts/shared_helpers/imu_preintegrator.md.

Excluded

• IMU sample acquisition / FC adapter integration — C8.
• Bias estimation / re-bias logic — C1, C5.
• Multi-threaded sample feeding — out of contract; helper is single-thread by design.
• Serialising preintegrated factors to FDR records — C13.
• The ImuSample / ImuWindow / ImuBias DTOs themselves — owned by _types/nav.py (AZ-263).

Acceptance Criteria

AC-1: Round-trip preintegration Given a synthetic IMU sequence of 100 samples with strictly-monotonic ts_ns When the producer calls integrate_sample 100 times then current_preintegration() Then a CombinedImuFactor is returned whose deltaTij equals the time span and whose delta_pose is non-zero

AC-2: Strict monotonicity rejects non-monotonic samples Given a preintegrator with the last integrated sample at ts_ns = T When integrate_sample(sample) is called with sample.ts_ns <= T Then ImuPreintegrationError is raised AND the preintegrator's internal accumulators are unchanged (a subsequent valid sample integrates as if the bad one never came)

AC-3: reset_for_new_keyframe is destructive Given a preintegrator with N integrated samples When reset_for_new_keyframe() is called Then the returned factor reflects all N samples AND a subsequent current_preintegration() (with no further samples) raises ImuPreintegrationError

AC-4: Re-bias affects subsequent samples only Given a sequence: reset_with_bias(bias_a), integrate 50 samples, reset_with_bias(bias_b), integrate 50 more When current_preintegration() is called Then the resulting factor reflects bias_a applied to samples 1–50 and bias_b applied to samples 51–100 (not bias_b retroactively)

AC-5: Determinism Given two instances constructed from the same calibration and fed the same (bias, samples) sequence When both call current_preintegration() Then the outputs are deep-equal

AC-6: Single-threaded, lock-free Given the helper's source code When inspected by the contract test (static analysis OR runtime reflection) Then no threading.Lock, RLock, Semaphore, or mutex is acquired anywhere in the integration path

AC-7: No upward imports (Layer 1 invariant) Given the helper module When a static-import check runs across gps_denied_onboard.helpers.imu_preintegrator Then it imports ONLY from _types, GTSAM, numpy, and stdlib — no gps_denied_onboard.components.* imports anywhere

Non-Functional Requirements

Performance

• integrate_sample p99 ≤ 200 µs on Tier-2 (Jetson Orin Nano Super) — overhead vs. inline GTSAM PIM ≤ 5 % (per E-CC-HELPERS hot-path NFR).
• current_preintegration p99 ≤ 100 µs on the same hardware.

Reliability

• Pure deterministic: same inputs → byte-equal CombinedImuFactor outputs.
• ImuPreintegrationError is the ONLY exception type the public surface raises on schema/timestamp violation; GTSAM's lower-level exceptions MUST be wrapped.

Unit Tests

AC Ref	What to Test	Required Outcome
AC-1	100 monotonic samples → current_preintegration()	factor deltaTij ≈ time span; non-zero delta_pose
AC-2	non-monotonic sample injection	ImuPreintegrationError; internal state unchanged (next valid sample integrates correctly)
AC-3	reset_for_new_keyframe then current_preintegration	second call raises ImuPreintegrationError (state cleared)
AC-4	re-bias mid-window	resulting factor distinguishes bias_a vs bias_b epochs
AC-5	two instances, same input	deep-equal factor outputs
AC-6	static / runtime lock check	no lock acquisition on the integration path
AC-7	importlinter / grep gate	no gps_denied_onboard.components.* imports
NFR-perf	microbench integrate_sample (10k iterations on Tier-2 fixture)	p99 ≤ 200 µs; overhead ≤ 5 %

Constraints

• Public surface frozen by _docs/02_document/contracts/shared_helpers/imu_preintegrator.md v1.0.0.
• Layer 1 Foundation only (per module-layout.md § Allowed Dependencies). NO upward imports.
• GTSAM is the single math backend — do not introduce a second IMU-preintegration library.
• No new dependency beyond what AZ-263 / E-BOOT pinned.

Risks & Mitigation

Risk 1: Concurrent calls from a misconfigured composition root silently corrupt the GTSAM PIM accumulator

• Risk: Two threads call integrate_sample simultaneously; GTSAM's PIM is not thread-safe; numerical drift goes undiagnosed.
• Mitigation: Helper is single-threaded by contract; the composition root binds one instance per writer thread. The contract test (AC-6) asserts no internal locking — making it a hard error if a future change tries to "make it thread-safe" instead of fixing the composition.

Risk 2: Sample-monotonicity-rejection silently masks an upstream FC clock skew

• Risk: A real IMU stream produces a non-monotonic sample (clock jitter); the helper rejects it; the consumer never learns.
• Mitigation: ImuPreintegrationError carries the offending vs. previous timestamp in its message so the consumer's catch-and-log path can record it as an FDR kind="imu.skew" event.

Runtime Completeness

• Named capability: GTSAM CombinedImuFactor preintegration via PreintegrationCombinedParams + PreintegratedCombinedMeasurements (architecture / E-CC-HELPERS / 01_helper_imu_preintegrator.md).
• Production code that must exist: real GTSAM-backed integration; real noise-model parsing from CameraCalibration; real strict-monotonic guard.
• Allowed external stubs: none — GTSAM is the production runtime.
• Unacceptable substitutes: pure-numpy "approximate" preintegration that ignores GTSAM's covariance propagation; deterministic-fallback that returns a zero factor; "for now we just integrate position with Euler" placeholder. Each would silently break C5's iSAM2 covariance honesty (AC-NEW-4).

Contract

This task produces the contract at _docs/02_document/contracts/shared_helpers/imu_preintegrator.md. Consumers MUST read that file — not this task spec — to discover the interface.