gps-denied-onboard/.planning/ROADMAP.md

# Roadmap: GPS-Denied Onboard Navigation System

## Overview

The scaffold exists (~2800 lines): FastAPI service, all component ABCs, Pydantic schemas, database layer, and SSE streaming are in place. What is missing is every algorithmic kernel. This roadmap implements them in dependency order: the ESKF math core first (everything else feeds into it), then the two sensor inputs (VO and satellite/GPR), then the MAVLink output that closes the loop to the flight controller, then end-to-end pipeline wiring, then a Docker SITL test harness, and finally accuracy validation against real flight data.

## Phases

- [ ] **Phase 1: ESKF Core** - 15-state error-state Kalman filter, coordinate transforms, confidence scoring
- [ ] **Phase 2: Visual Odometry** - cuVSLAM wrapper (Jetson) + OpenCV ORB stub (dev/CI) + TRT SuperPoint/LightGlue
- [ ] **Phase 3: Satellite Matching + GPR** - XFeat TRT matching, offline tile pipeline, real Faiss GPR index
- [ ] **Phase 4: MAVLink I/O** - pymavlink GPS_INPUT output loop, IMU input listener, telemetry, re-localization request
- [ ] **Phase 5: End-to-End Pipeline Wiring** - processor integration, GTSAM factor graph, recovery coordinator, object localization
- [ ] **Phase 6: Docker SITL Harness + CI** - ArduPilot SITL, camera replay, tile server mock, CI integration
- [ ] **Phase 7: Accuracy Validation** - 60-frame dataset validation, latency profiling, blackbox test suite

## Phase Details

### Phase 1: ESKF Core
**Goal**: A correct, standalone ESKF implementation exists that fuses IMU, VO, and satellite measurements and outputs confidence-tiered position estimates in WGS84
**Depends on**: Nothing (first phase — no other algorithmic component depends on this being absent)
**Requirements**: ESKF-01, ESKF-02, ESKF-03, ESKF-04, ESKF-05, ESKF-06
**Success Criteria** (what must be TRUE):
  1. ESKF propagates nominal state (position, velocity, quaternion, biases) from synthetic IMU inputs and covariance grows correctly between measurement updates
  2. VO measurement update reduces position uncertainty and innovation is within expected bounds for a simulated relative pose input
  3. Satellite measurement update corrects absolute position and covariance tightens to satellite noise level
  4. Confidence tier outputs HIGH when last satellite correction is recent and covariance is small, MEDIUM on VO-only, LOW on IMU-only — verified by unit tests
  5. Full coordinate chain (pixel → camera ray → body → NED → WGS84) produces correct GPS coordinates for a known geometry test case; all FAKE Math stubs replaced
**Plans**: 3 plans
Plans:
- [ ] 01-01-PLAN.md — ESKF core algorithm (schemas, 15-state filter, IMU prediction, VO/satellite updates, confidence tiers)
- [ ] 01-02-PLAN.md — Coordinate chain fix (replace fake math with real K matrix projection, ray-ground intersection)
- [ ] 01-03-PLAN.md — Unit tests for ESKF and coordinate chain (18+ ESKF tests, 10+ coordinate tests)

### Phase 2: Visual Odometry
**Goal**: VO produces metric relative poses via cuVSLAM on Jetson and via OpenCV ORB on dev/CI, both satisfying the same interface — no more scale-ambiguous unit vectors
**Depends on**: Phase 1 (ESKF provides metric scale reference and coordinate transforms for VO measurement update)
**Requirements**: VO-01, VO-02, VO-03, VO-04, VO-05
**Success Criteria** (what must be TRUE):
  1. cuVSLAM wrapper initializes in Inertial mode with camera intrinsics and IMU parameters, and returns RelativePose with `scale_ambiguous=False` and metric translation in NED
  2. OpenCV ORB stub satisfies the same ISequentialVisualOdometry interface and passes the same interface contract tests as the cuVSLAM wrapper
  3. TRT SuperPoint/LightGlue engines load and run inference on Jetson; MockInferenceEngine is selected automatically on dev/x86
  4. ImageInputPipeline accepts single-image batches without error; sequence lookup returns the correct frame with no substring collision
**Plans**: TBD

### Phase 3: Satellite Matching + GPR
**Goal**: The system can correct absolute position from pre-loaded satellite tiles and re-localize after tracking loss using a real Faiss descriptor index
**Depends on**: Phase 1 (ESKF position uncertainty drives tile selection radius and measurement update), Phase 2 (VO provides keyframe selection timing)
**Requirements**: SAT-01, SAT-02, SAT-03, SAT-04, SAT-05, GPR-01, GPR-02, GPR-03
**Success Criteria** (what must be TRUE):
  1. Satellite tile selection queries the local GeoHash-indexed directory using ESKF position ± 3σ and returns correct tiles without any HTTP requests
  2. Camera frame is GSD-normalized to satellite resolution before matching; XFeat TRT inference runs on Jetson and MockInferenceEngine on dev/CI
  3. RANSAC homography produces a WGS84 position estimate with a confidence score derived from inlier ratio, accepted by ESKF satellite measurement update
  4. GPR loads a real Faiss index from disk and returns tile candidates ranked by DINOv2 descriptor similarity (not random vectors)
  5. After simulated tracking loss, GPR candidate + MetricRefinement produces an ESKF re-localization within expected accuracy bounds
**Plans**: TBD

### Phase 4: MAVLink I/O
**Goal**: The flight controller receives GPS_INPUT at 5-10Hz and the system receives IMU data from the flight controller — the primary acceptance criterion is met end-to-end for the communication layer
**Depends on**: Phase 1 (ESKF state is the source for GPS_INPUT field population; IMU data drives ESKF prediction)
**Requirements**: MAV-01, MAV-02, MAV-03, MAV-04, MAV-05
**Success Criteria** (what must be TRUE):
  1. pymavlink sends GPS_INPUT messages to a MAVLink endpoint at 5-10Hz; all required fields populated (lat, lon, alt, velocity, accuracy, fix_type, hdop, vdop, GPS time)
  2. fix_type maps correctly from ESKF confidence tier: HIGH → 3 (3D fix), MEDIUM → 2 (2D fix), LOW → 0 (no fix)
  3. IMU listener receives ATTITUDE/RAW_IMU from flight controller at 5-10Hz and ESKF prediction step runs at that rate between camera frames
  4. After 3 consecutive frames with no position estimate, a MAVLink NAMED_VALUE_FLOAT message with last known position is sent (verifiable in SITL logs)
  5. Telemetry at 1Hz emits confidence score and drift estimate to ground station via NAMED_VALUE_FLOAT
**Plans**: TBD

### Phase 5: End-to-End Pipeline Wiring
**Goal**: A single uploaded camera frame travels through the full pipeline — VO, ESKF update, satellite correction (on keyframes), GPS_INPUT output — with no hardcoded stubs in the path
**Depends on**: Phase 1, Phase 2, Phase 3, Phase 4 (all algorithmic components must exist to be wired)
**Requirements**: PIPE-01, PIPE-02, PIPE-03, PIPE-04, PIPE-05, PIPE-06, PIPE-07, PIPE-08
**Success Criteria** (what must be TRUE):
  1. process_frame executes the full chain without error: VO relative pose → ESKF VO update → (every 5-10 frames) satellite match → ESKF satellite update → GPS_INPUT sent to flight controller
  2. SatelliteDataManager and CoordinateTransformer are instantiated in app.py lifespan and injected into the processor; no component is standalone
  3. FactorGraphOptimizer calls real GTSAM ISAM2 update when GTSAM is available; mock path remains for CI
  4. Object GPS localization (POST /objects/locate) returns a WGS84 position using the real pixel→ray→ground chain; hardcoded (48.0, 37.0) stub is gone
  5. Application starts without TypeError; ImageRotationManager constructor accepts the model manager argument
**Plans**: TBD

### Phase 6: Docker SITL Harness + CI
**Goal**: The full pipeline can be tested in a reproducible Docker environment with ArduPilot SITL, camera replay, and a tile server mock — and CI runs this on every commit
**Depends on**: Phase 5 (all components must be wired before integration testing is meaningful)
**Requirements**: TEST-01, TEST-02
**Success Criteria** (what must be TRUE):
  1. `docker compose up` starts ArduPilot SITL, the GPS-denied service, a camera-replay container, and a satellite tile server mock — all communicate over MAVLink and HTTP
  2. CI pipeline runs on x86 using OpenCV ORB stub and MockInferenceEngine; all 85+ unit tests pass with no manual steps
  3. MAVLink GPS_INPUT messages are captured in SITL logs and show 5-10Hz output rate during camera replay
  4. Tracking loss scenario (simulated by replaying frames with no overlap) triggers RECOVERY state and sends re-localization request
**Plans**: TBD

### Phase 7: Accuracy Validation
**Goal**: The system demonstrably meets the navigation accuracy acceptance criteria on the 60-frame test dataset, and all 21 blackbox test scenarios are implemented as runnable tests
**Depends on**: Phase 6 (SITL harness is required for the blackbox test scenarios)
**Requirements**: TEST-03, TEST-04, TEST-05
**Success Criteria** (what must be TRUE):
  1. Running against AD000001–AD000060.jpg with coordinates.csv ground truth: 80% of frames within 50m error and 60% of frames within 20m error
  2. Maximum cumulative VO drift between satellite corrections is less than 100m across any segment in the test dataset
  3. End-to-end latency per frame (camera capture to GPS_INPUT) is under 400ms on Jetson, with a breakdown report per pipeline stage
  4. All 21 blackbox test scenarios (FT-P-01 to FT-P-14, FT-N-01 to FT-N-07) run as pytest tests against the SITL harness and produce a pass/fail report
**Plans**: TBD

## Progress

| Phase | Plans Complete | Status | Completed |
|-------|----------------|--------|-----------|
| 1. ESKF Core | 0/3 | Planned | - |
| 2. Visual Odometry | 0/TBD | Not started | - |
| 3. Satellite Matching + GPR | 0/TBD | Not started | - |
| 4. MAVLink I/O | 0/TBD | Not started | - |
| 5. End-to-End Pipeline Wiring | 0/TBD | Not started | - |
| 6. Docker SITL Harness + CI | 0/TBD | Not started | - |
| 7. Accuracy Validation | 0/TBD | Not started | - |