GhostPilot: Build a GPS-Denied Drone Navigation Stack with Visual SLAM + Agentic AI

"Fly to the third floor, check each room for occupants, land at the helipad." — What if your drone could actually understand this?

By Aman Sachan | GitHub: AmSach/GhostPilot

---

🚀 What You'll Build

In this comprehensive guide, you'll build GhostPilot — an open-source drone navigation system that:

• Works without GPS using Visual-Inertial SLAM (VINS-Mono)
• Understands natural language missions via an LLM-based agent
• Navigates autonomously using ROS2 Nav2 stack
• Runs on edge hardware (Jetson Orin / Raspberry Pi 5)

---

📋 Table of Contents

1. The Problem: GPS is Fragile
2. System Architecture
3. Prerequisites & Setup
4. Part 1: Visual-Inertial SLAM
5. Part 2: Mission Parser (Agentic AI)
6. Part 3: Nav2 Integration & Pose Bridge
7. Part 4: End-to-End Simulation
8. Production Readiness Checklist
9. What's Next

---

The Problem: GPS is Fragile

Before we dive into code, let's understand why this matters.

Where GPS Fails

Environment	GPS Behavior	Impact
Indoors	No signal	Drones can't navigate buildings
Urban canyons	Multipath, 10-50m error	Unreliable for precision tasks
Forests	Canopy blocks signal	No coverage in wooded areas
Contested airspace	Jammed/spoofed	Military drones fail

Real-world context: Russia has jammed up to 85% of drones in some Ukraine sectors. GPS is not just unreliable — it's a single point of failure.

The $50K Problem

Current GPS-denied solutions are:

• Military systems: $50,000+ per unit
• Academic code: Unmaintained, undocumented
• Research papers: Theory without implementation

GhostPilot is the open-source answer.

---

System Architecture

GhostPilot is a three-layer stack:

┌─────────────────────────────────────────────┐
│  Layer 3: Agentic Mission Planner            │
│  "Fly to third floor, inspect rooms"         │
├─────────────────────────────────────────────┤
│  Layer 2: Visual-Inertial SLAM               │
│  Camera + IMU → 6DOF Pose                    │
├─────────────────────────────────────────────┤
│  Layer 1: Nav2 Navigation Stack              │
│  Path planning + Obstacle avoidance          │
└─────────────────────────────────────────────┘

Why This Separation Matters

Each layer can be tested independently:

# Test the parser without a drone
parser = MissionParser()
goals = parser.parse("Fly to floor 3, inspect area")
# Returns: [{"type": "NavigateToFloor", "floor": 3}, ...]

# Test SLAM without Nav2
slam = VINSMonoPipeline(config)
slam.process_frame(image, imu_data)
pose = slam.get_pose()  # Returns 6DOF pose

# Test the bridge without hardware
bridge = PoseBridge(max_jump_m=5.0)
accepted = bridge.process(pose)  # Rejects impossible jumps

---

Prerequisites & Setup

Hardware Options

Hardware	Cost	Performance	Recommended For
Jetson Orin AGX	$1,999	275 AI TOPS	Production deployment
Jetson Orin Nano	$499	40 AI TOPS	Development + light deployment
Raspberry Pi 5	$80	Limited	Learning + simulation
Laptop/Desktop	—	Good for dev	Development only

Software Stack

# Ubuntu 22.04 (recommended)
# ROS2 Humble
# Python 3.10+
# OpenCV 4.x

# Clone the repo
git clone https://github.com/AmSach/GhostPilot.git
cd GhostPilot

# Install Python dependencies
pip install -r requirements.txt

# Run headless simulation (no ROS2 required!)
python3 simulate.py

What You Get

GhostPilot/
├── src/
│   ├── ghostpilot_core/       # SLAM + Nav2 bridge
│   │   ├── vins_mono.py       # Pure-Python VINS-Mono estimator
│   │   ├── slam_node.py       # ROS2 wrapper
│   │   └── pose_bridge.py     # SLAM → Nav2 translator
│   ├── ghostpilot_agent/      # Mission parser + executor
│   │   ├── mission_parser.py  # Natural language → goals
│   │   └── executor.py        # Goal execution engine
│   └── ghostpilot_gazebo/     # Simulation environments
├── mock_ros2/                 # Test without ROS2 install
├── tests/                     # 63 automated tests
└── simulate.py                # End-to-end demo

---

Part 1: Visual-Inertial SLAM

What is SLAM?

SLAM = Simultaneous Localization And Mapping

The system answers two questions simultaneously:

1. Where am I? (Localization)
2. What does the world look like? (Mapping)

The VINS-Mono Pipeline

VINS-Mono is the gold standard for visual-inertial estimation. Here's how it works:

Camera Frames → Feature Tracking → IMU Pre-integration
       ↓                ↓                    ↓
   FAST corners    Optical Flow       Motion integration
       ↓                ↓                    ↓
       └────────────────┼────────────────────┘
                          ↓
              Sliding Window Optimization
                          ↓
                    6DOF Pose Estimate

Feature Tracking Implementation

# From vins_mono.py
class FeatureTracker:
    """
    Tracks visual features across frames using:
    - FAST corner detection
    - Pyramidal Lucas-Kanade optical flow
    - Forward-backward consistency check
    """

    def __init__(self, max_features=150):
        self.max_features = max_features
        self.feature_id = 0
        self.tracks = {}  # id → (point, age)

    def detect(self, image):
        """Detect FAST corners in the image."""
        # Convert to grayscale
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

        # Detect FAST corners
        corners = cv2.FAST_create(threshold=20).detect(gray)

        # Limit to max_features
        corners = sorted(corners, key=lambda x: -x.response)[:self.max_features]

        return np.array([[c.pt] for c in corners], dtype=np.float32)

    def track(self, prev_img, curr_img, prev_points):
        """Track points using Lucas-Kanade optical flow."""
        # Forward tracking
        next_points, status, _ = cv2.calcOpticalFlowPyrLK(
            prev_img, curr_img, prev_points, None
        )

        # Backward tracking (consistency check)
        back_points, back_status, _ = cv2.calcOpticalFlowPyrLK(
            curr_img, prev_img, next_points, None
        )

        # Filter: only keep consistent tracks
        fb_error = np.linalg.norm(back_points - prev_points, axis=1)
        valid = (status.flatten() == 1) & (back_status.flatten() == 1) & (fb_error < 1.0)

        return next_points[valid], prev_points[valid], valid

IMU Pre-integration

The IMU provides motion constraints between camera frames:

class IMUPreintegration:
    """
    Integrates IMU measurements between keyframes.

    The key insight: instead of storing every IMU sample,
    we pre-integrate them into a single motion constraint.
    """

    def __init__(self, gravity=9.81, gyro_noise=0.1, accel_noise=0.1):
        self.gravity = np.array([0, 0, gravity])
        self.gyro_noise = gyro_noise
        self.accel_noise = accel_noise

        # Pre-integrated state
        self.delta_R = np.eye(3)  # Rotation
        self.delta_v = np.zeros(3)  # Velocity
        self.delta_p = np.zeros(3)  # Position
        self.dt_sum = 0.0

    def integrate(self, accel, gyro, dt):
        """
        Integrate one IMU measurement.

        Args:
            accel: Acceleration [ax, ay, az] in m/s²
            gyro: Angular velocity [wx, wy, wz] in rad/s
            dt: Time since last measurement
        """
        # Mid-point integration for rotation
        gyro_mid = 0.5 * (gyro + gyro)  # Simplified
        dR = self._angle_to_rotation(gyro * dt)

        # Update rotation
        self.delta_R = self.delta_R @ dR

        # Update velocity and position
        # Note: This is simplified; full VINS uses Jacobians
        self.delta_v += self.delta_R @ accel * dt
        self.delta_p += self.delta_v * dt + 0.5 * (self.delta_R @ accel) * dt**2

        self.dt_sum += dt

    def _angle_to_rotation(self, angle_axis):
        """Convert angle-axis to rotation matrix."""
        angle = np.linalg.norm(angle_axis)
        if angle < 1e-10:
            return np.eye(3)
        axis = angle_axis / angle
        return cv2.Rodrigues(angle_axis)[0]

Sliding Window Optimization

Instead of optimizing the entire history, we keep a window of recent frames:

class SlidingWindowOptimizer:
    """
    Optimizes a sliding window of poses and landmarks.

    Key features:
    - Bounded computation (fixed window size)
    - Marginalization of old frames
    - Visual + IMU residuals
    """

    def __init__(self, window_size=10):
        self.window_size = window_size
        self.frames = []
        self.landmarks = {}

    def add_frame(self, frame):
        """Add a new frame to the window."""
        self.frames.append(frame)

        # If window too large, marginalize oldest frame
        if len(self.frames) > self.window_size:
            self._marginalize_oldest()

    def _marginalize_oldest(self):
        """
        Schur complement marginalization.

        Instead of discarding old frames, we compress their
        information into a prior on remaining frames.
        """
        oldest = self.frames.pop(0)

        # Build Schur complement (simplified)
        # In practice, this involves sparse matrix operations
        prior = self._compute_prior(oldest)

        # Add prior to remaining optimization
        self.prior_information = prior

    def optimize(self):
        """
        Run one optimization step.

        Minimizes:
        - Visual reprojection errors
        - IMU pre-integration residuals
        - Prior information (from marginalization)
        """
        # Build system matrix
        H = self._build_hessian()
        b = self._build_gradient()

        # Solve using Cholesky decomposition
        dx = np.linalg.solve(H, b)

        # Update poses
        for i, frame in enumerate(self.frames):
            frame.pose = self._update_pose(frame.pose, dx[i*7:(i+1)*7])

        return self.frames[0].pose if self.frames else None

Testing SLAM

# Run the tests
# tests/test_core.py

def test_quaternion_normalised():
    """Verify that SLAM output quaternion has unit norm."""
    pipeline = VINSMonoPipeline()

    for i in range(30):
        frame = generate_synthetic_frame(i)
        imu = generate_synthetic_imu(i)
        pipeline.process_frame(frame, imu)

    pose = pipeline.get_pose()
    q = pose[3:7]  # Quaternion part

    assert np.isclose(np.linalg.norm(q), 1.0), f"Quaternion norm: {np.linalg.norm(q)}"

def test_slam_initialises():
    """SLAM should initialise within first 5 frames."""
    pipeline = VINSMonoPipeline()

    for i in range(5):
        pipeline.process_frame(*generate_synthetic_data(i))

    assert pipeline.is_initialised(), "SLAM failed to initialise"

---

Part 2: Mission Parser (Agentic AI)

Natural Language → Structured Goals

The mission parser is the "brain" that understands operator intent:

Input:  "Fly to the 2nd floor, inspect the area, avoid personnel, report anomaly"
Output: [
    {"type": "NavigateToFloor", "floor": 2},
    {"type": "InspectArea", "area": "current"},
    {"type": "AvoidObstacle", "obstacle_type": "personnel"},
    {"type": "Report", "data": "anomaly"}
]

Dual-Mode Parser

The parser has two modes for reliability:

class MissionParser:
    """
    Natural language mission parser with dual-mode fallback.

    Mode 1: LLM-assisted (when available)
    Mode 2: Regex fallback (always available, deterministic)
    """

    def __init__(self, llm_available=False):
        self.llm_available = llm_available
        self.patterns = self._build_regex_patterns()

    def _build_regex_patterns(self):
        """Build deterministic regex patterns for common commands."""
        return {
            "floor": re.compile(
                r'(?:fly\s+to\s+)?(?:the\s+)?(\d+)(?:st|nd|rd|th)\s+floor|'
                r'(?:fly\s+to\s+)?(?:the\s+)?(first|second|third|fourth|fifth)\s+floor',
                re.IGNORECASE
            ),
            "inspect": re.compile(
                r'(?:inspect|check|scan)\s+(?:the\s+)?(?:area|room|building)',
                re.IGNORECASE
            ),
            "avoid": re.compile(
                r'(?:avoid|stay\s+away\s+from)\s+(personnel|people|obstacles|machinery)',
                re.IGNORECASE
            ),
            "land": re.compile(
                r'(?:land\s+at|return\s+to)\s+(?:the\s+)?(\w+)',
                re.IGNORECASE
            ),
            "report": re.compile(
                r'(?:report|notify)\s+(\w+)',
                re.IGNORECASE
            )
        }

    def parse(self, command: str) -> List[Dict]:
        """
        Parse a natural language command into structured goals.

        Args:
            command: Natural language mission command

        Returns:
            List of goal dictionaries
        """
        # Try LLM first if available
        if self.llm_available:
            try:
                return self._parse_with_llm(command)
            except Exception as e:
                print(f"LLM parsing failed: {e}, falling back to regex")

        # Always have regex fallback
        return self._parse_with_regex(command)

    def _parse_with_regex(self, command: str) -> List[Dict]:
        """Deterministic regex-based parsing."""
        goals = []

        # Floor navigation
        floor_match = self.patterns["floor"].search(command)
        if floor_match:
            floor = self._extract_floor(floor_match)
            goals.append({"type": "NavigateToFloor", "floor": floor})

        # Inspection
        if self.patterns["inspect"].search(command):
            goals.append({"type": "InspectArea", "area": "current"})

        # Avoidance
        avoid_match = self.patterns["avoid"].search(command)
        if avoid_match:
            goals.append({
                "type": "AvoidObstacle",
                "obstacle_type": avoid_match.group(1)
            })

        # Landing
        land_match = self.patterns["land"].search(command)
        if land_match:
            goals.append({
                "type": "LandAt",
                "location": land_match.group(1)
            })

        # Reporting
        report_match = self.patterns["report"].search(command)
        if report_match:
            goals.append({
                "type": "Report",
                "data": report_match.group(1)
            })

        return goals

    def _extract_floor(self, match) -> int:
        """Convert floor text to integer."""
        # Check for numeric ordinal
        if match.group(1):
            return int(match.group(1))

        # Check for word ordinal
        word_map = {
            "first": 1, "second": 2, "third": 3,
            "fourth": 4, "fifth": 5
        }
        return word_map.get(match.group(2).lower(), 1)

Why Regex Fallback Matters

In robotics, reliability > fancy features:

# Scenario: LLM is unavailable (offline deployment, API down)
# Regex still works!

parser = MissionParser(llm_available=False)

# These all work:
parser.parse("Fly to 3rd floor")
parser.parse("Go to the second floor and check the rooms")
parser.parse("Avoid personnel, inspect area, report damage")

Mission Executor

The executor runs goals sequentially with proper error handling:

class MissionExecutor:
    """
    Executes parsed mission goals with:
    - Sequential goal processing
    - Success/failure tracking
    - Nav2 integration
    """

    def __init__(self, nav2_client=None):
        self.nav2 = nav2_client
        self.mission_log = []

    def execute(self, goals: List[Dict]) -> Dict:
        """
        Execute a list of goals.

        Returns mission report with success/failure status.
        """
        results = []

        for goal in goals:
            result = self._execute_goal(goal)
            results.append({
                "goal": goal,
                "success": result["success"],
                "message": result.get("message", "")
            })

            # Log each goal result
            self.mission_log.append(result)

        return {
            "completed": all(r["success"] for r in results),
            "results": results
        }

    def _execute_goal(self, goal: Dict) -> Dict:
        """Execute a single goal."""
        goal_type = goal["type"]

        handlers = {
            "NavigateToFloor": self._navigate_to_floor,
            "InspectArea": self._inspect_area,
            "AvoidObstacle": self._avoid_obstacle,
            "LandAt": self._land_at,
            "Report": self._send_report
        }

        handler = handlers.get(goal_type)
        if not handler:
            return {"success": False, "message": f"Unknown goal type: {goal_type}"}

        return handler(goal)

    def _navigate_to_floor(self, goal: Dict) -> Dict:
        """Navigate to a specific floor (converts to altitude)."""
        floor = goal["floor"]
        altitude = floor * 3.0  # 3m per floor (configurable)

        # Send to Nav2
        if self.nav2:
            success = self.nav2.go_to_altitude(altitude)
            return {"success": success, "altitude": altitude}

        # Mock mode
        return {"success": True, "altitude": altitude, "mock": True}

    def _avoid_obstacle(self, goal: Dict) -> Dict:
        """Configure obstacle avoidance."""
        obstacle_type = goal.get("obstacle_type", "unknown")

        # Map obstacle types to inflation radii
        inflation_map = {
            "personnel": 2.0,   # 2m safety buffer for people
            "people": 2.0,
            "machinery": 3.0,  # 3m for equipment
            "obstacles": 1.5,  # Default
            "unknown": 1.0
        }

        radius = inflation_map.get(obstacle_type, 1.0)

        # Update Nav2 costmap
        if self.nav2:
            self.nav2.set_inflation_radius(radius)

        return {"success": True, "inflation_radius": radius}

---

Part 3: Nav2 Integration & Pose Bridge

The Pose Bridge: SLAM → Nav2

Nav2 expects localization in a specific format. The pose bridge is the translator:

class PoseBridge:
    """
    Converts SLAM pose output to Nav2-compatible localization.

    Key features:
    - Frame transformation
    - Velocity estimation
    - Jump rejection (safety!)
    - Odometry publishing
    """

    def __init__(self, max_jump_meters=5.0, frame_map="map", frame_base="base_link"):
        self.max_jump = max_jump_meters
        self.frame_map = frame_map
        self.frame_base = frame_base

        self.prev_pose = None
        self.prev_time = None

    def process(self, pose: np.ndarray, timestamp: float = None) -> Dict:
        """
        Process a SLAM pose estimate.

        Args:
            pose: 7D pose [x, y, z, qw, qx, qy, qz]
            timestamp: Current time (for velocity estimation)

        Returns:
            Processed localization data, or None if rejected
        """
        # Check for reasonable pose
        if not self._is_valid_pose(pose):
            return {"accepted": False, "reason": "invalid_pose"}

        # Jump rejection
        if self.prev_pose is not None:
            jump = np.linalg.norm(pose[:3] - self.prev_pose[:3])
            if jump > self.max_jump:
                print(f"⚠️ Rejected pose jump: {jump:.1f}m (max: {self.max_jump}m)")
                return {"accepted": False, "reason": "jump_too_large", "jump": jump}

        # Compute velocity
        velocity = self._estimate_velocity(pose, timestamp)

        # Store for next iteration
        self.prev_pose = pose.copy()
        self.prev_time = timestamp

        return {
            "accepted": True,
            "pose": pose,
            "velocity": velocity,
            "frame_map": self.frame_map,
            "frame_base": self.frame_base
        }

    def _is_valid_pose(self, pose: np.ndarray) -> bool:
        """Check if pose is numerically valid."""
        # Check for NaN/Inf
        if not np.all(np.isfinite(pose)):
            return False

        # Check quaternion normalization
        q_norm = np.linalg.norm(pose[3:7])
        if not np.isclose(q_norm, 1.0, atol=0.01):
            print(f"⚠️ Quaternion not normalized: {q_norm}")
            return False

        return True

    def _estimate_velocity(self, pose: np.ndarray, timestamp: float) -> np.ndarray:
        """Estimate velocity from successive poses."""
        if self.prev_pose is None or self.prev_time is None:
            return np.zeros(6)

        dt = timestamp - self.prev_time
        if dt <= 0:
            return np.zeros(6)

        # Linear velocity
        v_linear = (pose[:3] - self.prev_pose[:3]) / dt

        # Angular velocity (simplified)
        v_angular = np.zeros(3)  # Would compute from quaternion derivative

        return np.concatenate([v_linear, v_angular])

Why Jump Rejection Saves Lives

# Scenario: SLAM glitch causes 19.8m jump in 1 second
bridge = PoseBridge(max_jump_meters=5.0)

# Normal pose
result1 = bridge.process(np.array([0, 0, 0, 1, 0, 0, 0]), t=0.0)
# → accepted: True

# Glitch: 19.8m jump!
result2 = bridge.process(np.array([19.8, 0, 0, 1, 0, 0, 0]), t=1.0)
# → accepted: False, reason: "jump_too_large", jump: 19.8

# Nav2 never sees the bad estimate — system stays stable

---

Part 4: End-to-End Simulation

Running the Full Stack

# Headless simulation (no ROS2 required!)
python3 simulate.py

What Happens in the Simulation

# simulate.py - simplified

def run_simulation():
    # 1. Initialize components
    parser = MissionParser()
    executor = MissionExecutor()
    slam = VINSMonoPipeline()
    bridge = PoseBridge()

    # 2. Parse a mission
    command = "Fly to 2nd floor, inspect area, avoid personnel, report anomaly"
    goals = parser.parse(command)
    print(f"Parsed goals: {goals}")

    # 3. Simulate SLAM convergence
    for i in range(30):
        frame, imu = generate_synthetic_data(i)
        slam.process_frame(frame, imu)

        if slam.is_initialised():
            pose = slam.get_pose()
            result = bridge.process(pose, timestamp=i*0.1)

            if result["accepted"]:
                executor.update_localization(result)

    # 4. Execute mission
    report = executor.execute(goals)

    # 5. Output results
    print(f"\n{'='*50}")
    print(f"Mission completed: {report['completed']}")
    print(f"Final altitude: {slam.get_pose()[2]:.1f}m (expected: {2*3.0}m)")
    print(f"{'='*50}")

    return report

Expected Output

Parsed goals: [
    {'type': 'NavigateToFloor', 'floor': 2},
    {'type': 'InspectArea', 'area': 'current'},
    {'type': 'AvoidObstacle', 'obstacle_type': 'personnel'},
    {'type': 'Report', 'data': 'anomaly'}
]

[SLAM] Initialised at frame 4
[POSE] Accepted pose: [0.0, 0.0, 0.1]
[POSE] Rejected pose jump: 19.8m (max: 5.0m)
[EXEC] Navigating to floor 2 (altitude: 6.0m)
[EXEC] Inspecting area
[EXEC] Avoiding personnel (radius: 2.0m)
[EXEC] Reporting anomaly

==================================================
Mission completed: True
Final altitude: 6.0m (expected: 6.0m)
==================================================

---

Production Readiness Checklist

✅ What's Working

Component	Status	Tests
Mission Parser	✅ Production	10/10 passing
Mission Executor	✅ Production	8/8 passing
VINS-Mono Pipeline	✅ Tested	25/25 passing
Pose Bridge	✅ Production	7/7 passing
Headless Simulation	✅ Working	End-to-end verified
Jump Rejection	✅ Safety critical	Verified

⚠️ Needs Real Hardware

Component	Status	Required Action
Camera/IMU Calibration	⚠️ Not done	Run calibrate_camera.sh on hardware
Nav2 Real Integration	⚠️ Mock only	Deploy on ROS2 Humble + Nav2
PX4/MAVLink	⚠️ Not tested	Connect flight controller
Outdoor Flight Test	❌ TODO	Field validation
Multi-drone Coordination	❌ TODO	Future roadmap

🔧 Before Real Flight

1. Calibrate sensors: ./scripts/calibrate_camera.sh
2. Install ROS2 Humble: Full Nav2 stack required
3. Connect hardware: RealSense + PX4 + drone frame
4. Safety pilot: Manual override via RC controller
5. Regulatory compliance: Follow local drone laws

---

What's Next

Roadmap

• [ ] Complete VINS-Mono C++ integration for performance
• [ ] Add ORB-SLAM3 as alternative SLAM backend
• [ ] Multi-drone coordination protocols
• [ ] Real hardware testing with RealSense + PX4
• [ ] Simulation environments for common scenarios

Contributing

Priority areas for contributors:

1. VINS-Mono / ORB-SLAM3 integration — Make it fly on real hardware
2. Hardware testing guides — Calibration, deployment
3. Simulation scenarios — Indoor, urban, forest environments

Links

• GitHub: github.com/AmSach/GhostPilot
• Author: Aman Sachan | Instagram
• Papers: See research-paper.md and technical-paper.md in the repo

---

📚 Further Reading

If you want to dive deeper:

1. VINS-Mono Paper: Qin et al., "A Robust and Versatile Monocular Visual-Inertial State Estimator" (IEEE T-RO, 2018)
2. ORB-SLAM3: Campos et al., "An Accurate Open-Source SLAM System" (IEEE T-RO, 2021)
3. Nav2 Documentation: navigation.ros.org
4. ROS2 Humble: docs.ros.org/en/humble

---

GhostPilot proves that GPS-denied drone navigation can be open, understandable, and testable — without sacrificing the serious robotics underneath.

Star the repo ⭐ if you found this useful!

---

Aman Sachan builds open-source robotics and AI systems. Follow his work on GitHub and LinkedIn.