# GPS-Denied Navigation

How the system navigates without GPS.

## Principle

All navigation uses relative positioning from visual sensors. GPS is only used for geofencing (safety boundaries).

| Function | GPS Used? |
|----------|-----------|
| Position estimation | No - visual odometry |
| Waypoint navigation | No - local coordinates |
| Velocity control | No - optical flow |
| Geofencing | Yes - safety only |

## Position Estimation

### Visual Odometry

1. Detect features in camera image (ORB, SIFT)
2. Match features between consecutive frames
3. Estimate camera motion from feature displacement
4. Accumulate motion into position estimate

### Optical Flow

1. Capture ground images from downward camera
2. Measure pixel displacement between frames
3. Convert to velocity using altitude
4. Integrate for position

### Sensor Fusion

Extended Kalman Filter combines:
- Visual odometry (position)
- Optical flow (velocity)
- IMU (acceleration, rotation)
- Barometer (altitude)

Output: Full 6-DOF pose estimate

## ArduPilot Configuration

Key parameters for GPS-denied operation:

```
# EKF Source Configuration
EK3_SRC1_POSXY = 6    # External Nav for position
EK3_SRC1_VELXY = 6    # External Nav for velocity
EK3_SRC1_POSZ = 1     # Barometer for altitude

# Disable GPS for navigation
GPS_TYPE = 0          # No GPS (or keep for geofence)

# Enable external navigation
VISO_TYPE = 1         # Enable visual odometry input

# Arming checks
ARMING_CHECK = 0      # Disable pre-arm checks (for testing)
```

See `config/ardupilot_gps_denied.parm` for complete parameters.

## Sending Position to ArduPilot

Visual odometry sends position via MAVLink:

```python
# VISION_POSITION_ESTIMATE message
msg = mavutil.mavlink.MAVLink_vision_position_estimate_message(
    usec=timestamp_us,
    x=position_x,      # meters, NED frame
    y=position_y,
    z=position_z,
    roll=roll,         # radians
    pitch=pitch,
    yaw=yaw
)
```

## Drift Mitigation

Visual odometry accumulates drift over time. Strategies:

1. **Loop Closure**: Recognize previously visited locations
2. **Landmark Matching**: Use known visual markers
3. **Multi-Sensor Fusion**: Weight sensors by confidence
4. **Periodic Reset**: Return to known position

## Geofencing

GPS is only used for safety boundaries:

```yaml
geofence:
  enabled: true
  use_gps: true
  fence_type: polygon
  action: RTL
  max_altitude: 50
```

If drone crosses boundary, triggers return-to-launch.

## Coordinate System

All waypoints use local NED coordinates:
- X: North (meters from origin)
- Y: East (meters from origin)
- Z: Down (negative for altitude)

Example mission:
```python
waypoints = [
    {"x": 0, "y": 0, "z": -5},    # Takeoff to 5m
    {"x": 10, "y": 0, "z": -5},   # 10m north
    {"x": 10, "y": 10, "z": -5},  # 10m east
    {"x": 0, "y": 0, "z": -5},    # Return
    {"x": 0, "y": 0, "z": 0},     # Land
]
```

## Limitations

- Drift accumulates over distance/time
- Requires visual features (fails in featureless environments)
- Requires sufficient lighting
- Performance degrades with fast motion or blur