simulation/docs/gps_denied_navigation.md

GPS-Denied Navigation

Overview

This system enables UAV/UGV navigation without GPS by using:

1. Visual Odometry - Camera-based pose estimation
2. Optical Flow - Velocity estimation from downward camera
3. IMU Integration - Short-term dead reckoning
4. EKF Fusion - Combine all sensors

GPS is ONLY used for geofencing (safety boundaries).

How It Works

Visual Odometry

Frame N-1    Frame N
    │           │
    ▼           ▼
┌────────┐  ┌────────┐
│Features│──│Features│  → Match features
└────────┘  └────────┘
    │           │
    └─────┬─────┘
          ▼
    Essential Matrix → Rotation + Translation

Algorithm:

1. Detect ORB/SIFT features in current frame
2. Match with previous frame features
3. Compute Essential Matrix (RANSAC)
4. Recover rotation and translation
5. Integrate to get absolute pose

Optical Flow

Downward Camera
      │
      ▼
  ┌───────┐
  │ Image │ → Lucas-Kanade flow
  └───────┘
      │
      ▼
  Pixel velocity × Altitude / Focal length = Ground velocity

Works best at:

• Altitudes 0.5m - 10m
• Textured ground surfaces
• Stable lighting

Sensor Fusion

Visual Odometry ─┬─→ Weighted Average ─→ Position Estimate
Optical Flow ────┤
IMU ─────────────┘

Weights (configurable):

• Visual Odometry: 60%
• Optical Flow: 30%
• IMU: 10%

ArduPilot Configuration

EKF3 External Navigation

# GPS Type - Disabled
GPS_TYPE 0
GPS_TYPE2 0

# EKF3 Source Configuration
AHRS_EKF_TYPE 3      # Use EKF3
EK3_ENABLE 1
EK2_ENABLE 0

# Position from External Nav
EK3_SRC1_POSXY 6     # External Nav
EK3_SRC1_POSZ 1      # Barometer
EK3_SRC1_VELXY 6     # External Nav
EK3_SRC1_VELZ 0      # None
EK3_SRC1_YAW 6       # External Nav

# Vision Position Input
VISO_TYPE 1          # MAVLink
VISO_POS_X 0.1       # Camera offset
VISO_DELAY_MS 50     # Processing delay

Arming Checks

# For simulation, disable all
ARMING_CHECK 0

# For real flight, keep safety checks
# ARMING_CHECK 14  # Skip GPS only

Coordinate Frames

Local NED Frame

        North (X+)
           ↑
           │
           │
West ←─────┼─────→ East (Y+)
           │
           │
           ↓
        (Down is Z+)

All navigation uses LOCAL coordinates:

• Takeoff point is origin (0, 0, 0)
• No global GPS coordinates
• Relative waypoints only

Example Waypoints

# Square pattern (5m sides)
waypoints = [
    (5, 0, -5),    # North 5m, altitude 5m
    (5, 5, -5),    # North-East corner
    (0, 5, -5),    # East
    (0, 0, -5),    # Back to start
]

Limitations

Visual Odometry

• Scale drift: Position error grows over time
• Texture needed: Poor in featureless environments
• Lighting: Affected by shadows, brightness changes
• Motion blur: High-speed motion degrades accuracy

Optical Flow

• Altitude dependent: Accuracy varies with height
• Ground texture: Needs visible ground features
• Tilt sensitivity: Assumes mostly horizontal flight

Mitigation Strategies

1. Loop closure: Return to known positions
2. Landmark detection: ArUco markers for correction
3. Multi-sensor fusion: Combine VO + OF + IMU
4. Flight patterns: Minimize cumulative drift

Geofencing (GPS Only)

GPS is used only for safety:

# Geofence uses GPS coordinates
fence_points = [
    (47.397742, 8.545594),  # Corner 1 (lat, lon)
    (47.398242, 8.545594),  # Corner 2
    (47.398242, 8.546094),  # Corner 3
    (47.397742, 8.546094),  # Corner 4
]

# But navigation uses local coordinates
current_position = (10.5, 3.2, -5.0)  # NED, meters

Breach Actions:

• RTL - Return to local origin
• LAND - Land immediately
• HOLD - Hold position

Testing GPS-Denied Mode

1. Verify EKF Source

In MAVProxy:

param show EK3_SRC*

Should show:

EK3_SRC1_POSXY    6.0
EK3_SRC1_VELXY    6.0

2. Check Vision Input

ros2 topic echo /uav/visual_odometry/pose

Should show updating position.

3. Monitor EKF Status

ros2 topic echo /uav/mavros/state

Should show connected: true and mode: GUIDED.