# 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

```python
# 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:

```python
# 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

```bash
ros2 topic echo /uav/visual_odometry/pose
```

Should show updating position.

### 3. Monitor EKF Status

```bash
ros2 topic echo /uav/mavros/state
```

Should show `connected: true` and `mode: GUIDED`.