mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-23 11:16:37 +00:00
feat(phases 2-7): implement full GPS-denied navigation pipeline
Phase 2 — Visual Odometry: - ORBVisualOdometry (dev/CI), CuVSLAMVisualOdometry (Jetson) - TRTInferenceEngine (TensorRT FP16, conditional import) - create_vo_backend() factory Phase 3 — Satellite Matching + GPR: - SatelliteDataManager: local z/x/y tiles, ESKF ±3σ tile selection - GSD normalization (SAT-03), RANSAC inlier-ratio confidence (SAT-04) - GlobalPlaceRecognition: Faiss index + numpy fallback Phase 4 — MAVLink I/O: - MAVLinkBridge: GPS_INPUT 15+ fields, IMU callback, 1Hz telemetry - 3-consecutive-failure reloc request - MockMAVConnection for CI Phase 5 — Pipeline Wiring: - ESKF wired into process_frame: VO update → satellite update - CoordinateTransformer + SatelliteDataManager via DI - MAVLink state push per frame (PIPE-07) - Real pixel_to_gps via ray-ground projection (PIPE-06) - GTSAM ISAM2 update when available (PIPE-03) Phase 6 — Docker + CI: - Multi-stage Dockerfile (python:3.11-slim) - docker-compose.yml (dev), docker-compose.sitl.yml (ArduPilot SITL) - GitHub Actions: ci.yml (lint+pytest+docker smoke), sitl.yml (nightly) - tests/test_sitl_integration.py (8 tests, skip without SITL) Phase 7 — Accuracy Validation: - AccuracyBenchmark + SyntheticTrajectory - AC-PERF-1: 80% within 50m ✅ - AC-PERF-2: 60% within 20m ✅ - AC-PERF-3: p95 latency < 400ms ✅ - AC-PERF-4: VO drift 1km < 100m ✅ (actual ~11m) - scripts/benchmark_accuracy.py CLI Tests: 195 passed / 8 skipped Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,42 @@
|
||||
---
|
||||
plan: 01-01
|
||||
phase: 01-eskf-core
|
||||
status: complete
|
||||
started: 2026-04-01T20:36:07Z
|
||||
completed: 2026-04-01T20:45:00Z
|
||||
---
|
||||
|
||||
## Summary
|
||||
|
||||
Implemented the 15-state Error-State Kalman Filter (ESKF) from scratch using NumPy only. Covers ESKF-01 through ESKF-05.
|
||||
|
||||
## Key Files Created
|
||||
|
||||
- `src/gps_denied/schemas/eskf.py` (68 lines) — ConfidenceTier, IMUMeasurement, ESKFConfig, ESKFState schemas
|
||||
- `src/gps_denied/core/eskf.py` (359 lines) — ESKF class with predict(), update_vo(), update_satellite(), get_confidence(), initialize_from_gps()
|
||||
|
||||
## Commits
|
||||
|
||||
- `57c7a6b` feat(eskf): add ESKF schema contracts (ESKF-01, ESKF-04, ESKF-05)
|
||||
- `9d5337a` feat(eskf): implement 15-state ESKF core algorithm (ESKF-01..05)
|
||||
|
||||
## Tasks Completed
|
||||
|
||||
| Task | Status | Notes |
|
||||
|------|--------|-------|
|
||||
| Task 1: ESKF schema contracts | ✓ | All 4 classes importable |
|
||||
| Task 2: ESKF core algorithm | ✓ | All acceptance criteria passed |
|
||||
|
||||
## Deviations
|
||||
|
||||
None — implemented as specified in plan.
|
||||
|
||||
## Self-Check: PASSED
|
||||
|
||||
- [x] ESKF class importable
|
||||
- [x] predict() propagates state and grows covariance
|
||||
- [x] update_vo() reduces position uncertainty via Kalman gain
|
||||
- [x] update_satellite() corrects absolute position
|
||||
- [x] get_confidence() returns correct tier
|
||||
- [x] initialize_from_gps() uses CoordinateTransformer
|
||||
- [x] All math NumPy only, no external filter library
|
||||
@@ -0,0 +1,38 @@
|
||||
---
|
||||
plan: 01-02
|
||||
phase: 01-eskf-core
|
||||
status: complete
|
||||
completed: 2026-04-01T20:50:00Z
|
||||
---
|
||||
|
||||
## Summary
|
||||
|
||||
Replaced all FAKE Math stubs in CoordinateTransformer with real camera projection mathematics. Implements the complete pixel-to-GPS chain via K^-1 unprojection, camera-to-body rotation, quaternion body-to-ENU transformation, and ray-ground intersection (ESKF-06).
|
||||
|
||||
## Key File Modified
|
||||
|
||||
- `src/gps_denied/core/coordinates.py` (176 lines added) — Real K matrix math, helper functions, pixel_to_gps, gps_to_pixel, cv2.perspectiveTransform
|
||||
|
||||
## Commits
|
||||
|
||||
- `dccadd4` feat(coordinates): implement real pixel-to-GPS projection chain (ESKF-06)
|
||||
|
||||
## New Helpers
|
||||
|
||||
- `_build_intrinsic_matrix()` — K matrix from focal_length, sensor size, resolution
|
||||
- `_cam_to_body_rotation()` — Rx(180deg) for nadir-pointing camera
|
||||
- `_quat_to_rotation_matrix()` — Quaternion to 3x3 rotation matrix
|
||||
|
||||
## Deviations
|
||||
|
||||
None — all acceptance criteria passed.
|
||||
|
||||
## Self-Check: PASSED
|
||||
|
||||
- [x] K^-1 unprojection replaces 0.1m/pixel fake scaling
|
||||
- [x] Ray-ground intersection at altitude
|
||||
- [x] gps_to_pixel is exact inverse
|
||||
- [x] transform_points uses cv2.perspectiveTransform
|
||||
- [x] All FAKE Math stubs removed
|
||||
- [x] Image center pixel projects to UAV nadir
|
||||
- [x] Backward-compatible defaults (ADTI 20L V1)
|
||||
Reference in New Issue
Block a user