Aerial Position Estimation using Non-GPS Computer Vision

A real-time drone position estimation system that uses computer vision and feature matching to determine drone pose relative to a satellite reference image, eliminating the need for GPS positioning.

🎯 Overview

This system enables autonomous drone navigation in GPS-denied environments by:

• Capturing live video feed from drone camera
• Matching visual features between drone camera feed and satellite reference image
• Calculating real-time position (X, Y coordinates) relative to the reference map
• Sending position estimates to ArduPilot flight controller via MAVLink

🔧 System Architecture

┌─────────────────┐    ┌──────────────────┐    ┌─────────────────────┐
│   Drone Camera  │───▶│  Feature Matching │───▶│  Position Estimate  │
│   (Live Feed)   │    │   & Homography    │    │   (X, Y in meters)  │
└─────────────────┘    └──────────────────┘    └─────────────────────┘
                                │                           │
                                ▼                           ▼
┌─────────────────┐    ┌──────────────────┐    ┌─────────────────────┐
│ Satellite Image │    │  SIFT Features   │    │   MAVLink Message   │
│  (Reference)    │    │   (20k points)   │    │  to Flight Control │
└─────────────────┘    └──────────────────┘    └─────────────────────┘

🧠 How Drone Pose Calculation Works

1. Initialization Phase

• Reference Image Processing: Loads satellite_image-main.png and extracts 20,000 SIFT features
• MAVLink Connection: Establishes connection to ArduPilot on tcp:127.0.0.1:5762
• Video Stream Setup: Connects to drone camera via UDP stream on port 5600

2. Takeoff and Initial Position

while not self.check_takeoff_complete(10):
    # Wait for drone to reach 10m altitude
    self.start_pos = self.checkStart.compParam("satellite_image-main.png", self.frame)

• Waits for drone to reach 10-meter target altitude
• Establishes initial position reference for relative positioning

3. Real-time Position Estimation Loop

Feature Detection & Matching

# Extract SIFT features from current frame
sift = cv.SIFT_create(nfeatures=20000)
kp2, des2 = sift.detectAndCompute(current_frame, None)

# Match features using FLANN matcher
matches = flann.knnMatch(reference_features, current_features, k=2)

Homography Calculation

# Find geometric transformation between images
M, mask = cv.findHomography(src_pts, dst_pts, cv.RANSAC, 5.0)

Position Calculation Formula

# Convert pixel offset to real-world coordinates
self.inCMX = (x - reference_center_x) * altitude / focal_length
self.inCMY = (y - reference_center_y) * altitude / focal_length

Where:

• x, y: Center of matched region in reference image
• altitude: Current drone altitude from barometer
• focal_length: Camera focal length (25.74 pixels/meter at 1m altitude)
• inCMX, inCMY: Position offset in meters from reference point

4. Position Broadcasting

def setPos(self, x=0, y=0, z=0):
    the_connection.mav.send(mavutil.mavlink.MAVLink_vision_position_estimate_message(
        10, x, y, 0, 0, 0, 0,
        [1,0,0,0,0,0,1,0,0,0,0,1,0,0,0,1,0,0,1,0,1], 0
    ))

Sends calculated position to ArduPilot as VISION_POSITION_ESTIMATE message.

📦 Installation and Setup

Prerequisites

• Python 3.8+
• OpenCV with GStreamer support
• ArduPilot SITL or physical drone with MAVLink
• Gazebo simulator (for testing)

Quick Setup

You can clone this repo to $HOME and run the ./setup.sh script directly to set it all up at once (give root access if required (sudo)).

chmod +x setup.sh
./setup.sh

Manual Setup

1. Setup ardupilot_gazebo

The first step assumes you have built the ArduPilotPlugin and got ardupilot_gazebo setup on the system. Follow the instructions provided here if not.

2. Configure Environment Variables

Set the Gazebo environment variables in your .bashrc or .zshrc or in the terminal used to run Gazebo.

Assuming that you have cloned the repository to $HOME/ap_nongps:

export GZ_SIM_RESOURCE_PATH=$HOME/ap_nongps/models:$HOME/ap_nongps/worlds:$GZ_SIM_RESOURCE_PATH

For permanent setup in .bashrc or .zshrc:

echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ap_nongps/models:$HOME/ap_nongps/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc

Reload your terminal with source ~/.bashrc.

3. Install Dependencies

For Ubuntu:

sudo apt-get install libgirepository1.0-dev libcairo2-dev
sudo apt-get install gobject-introspection

For macOS:

brew install cairo
brew install gobject-introspection
brew install inih

Install Python requirements:

pip install -r requirements.txt

🚁 Usage

Terminal 1: Start Gazebo Simulation

For Gazebo Harmonic / Garden:

gz sim -v4 -r iris_runway_ngps.sdf

Start streaming:

gz topic -t /world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image/enable_streaming -m gz.msgs.Boolean -p "data: 1"

The terminal used to launch Gazebo should display the following if streaming started correctly:

[Msg] GstCameraPlugin:: streaming: started
[Dbg] [GstCameraPlugin.cc:407] GstCameraPlugin: creating generic pipeline
[Msg] GstCameraPlugin: GStreamer element set state returned: 2
[Msg] GstCameraPlugin: starting GStreamer main loop

Terminal 2: Start ArduPilot SITL

cd ardupilot && sim_vehicle.py -D -v ArduCopter -f JSON --add-param-file=$HOME/ardupilot_gazebo_ap/config/gazebo-iris-gimbal-ngps.parm --console --map

Takeoff 10m (hardcoded for the time being):

mode GUIDED
arm throttle force    # Force arm because visual odometry reports unhealthy initially
takeoff 10

Set gimbal parameters:

rc 6 1500
rc 7 1300
rc 8 1500

Terminal 3: Run Position Estimation

cd src && python video_to_feature.py

Expected Output

If everything's working, you'll see output like:

$ python video_to_feature.py
Heartbeat from system (system 1 component 0)
Offset x, y(in cms):  0.1942269262460972 -0.3884538524921944
Offset x, y(in cms):  0.1942269262460972 -0.3884538524921944

The system will also display two windows:

• "Drone Live Feed": Raw camera stream
• "Feature Matching": Processed frames with detected features

Press 'Q' to exit gracefully.

📁 File Structure

├── src/
│   ├── video_to_feature.py       # Main execution script
│   ├── feature_match_ardu.py     # ArduPilot feature matching
│   ├── feature_match_standalone.py # Standalone feature matching
│   ├── video_capture_gazebo.py   # GStreamer video capture
│   ├── base_structures.py        # Basic data structures
│   └── satellite_image-main.png  # Reference satellite image
├── models/arena/                 # Gazebo world models
├── worlds/                       # Gazebo world files
├── requirements.txt              # Python dependencies
└── setup.sh                     # Environment setup script

🔧 Configuration

Camera Parameters

self.focal = 25.7430836014194  # Focal length calibration

Feature Detection Settings

self.MIN_MATCH_COUNT = 10      # Minimum features for valid match
nfeatures = 20000              # Maximum SIFT features to detect

Connection Settings

mavlink_connection('tcp:127.0.0.1:5762')  # MAVLink connection
udpsrc port=5600                           # Video stream port

📊 Technical Details

Computer Vision Pipeline

1. SIFT Feature Detection: Scale-Invariant Feature Transform for robust feature detection
2. FLANN Matching: Fast Library for Approximate Nearest Neighbors for efficient matching
3. RANSAC Homography: RANdom SAmple Consensus for outlier-robust transformation estimation
4. Perspective Transformation: Geometric mapping between image coordinate systems

Coordinate System

• Image Coordinates: Pixel-based (0,0 at top-left)
• World Coordinates: Meter-based relative to reference image center
• MAVLink NED: North-East-Down coordinate frame for flight controller