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

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 The Problem: GPS is Fragile System Architecture Prerequisites & Setup Part 1: Visual-Inertial SLAM Part 2: Mission Parser (Agentic AI) Part 3: Nav2 Integration & Pose Bridge Part 4: End-to-End Simulation Production Readiness Checklist 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: Where am I? (Localization) 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 Calibrate sensors: ./scripts/calibrate_camera.sh Install ROS2 Humble: Full Nav2 stack required Connect hardware: RealSense + PX4 + drone frame Safety pilot: Manual override via RC controller 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: VINS-Mono / ORB-SLAM3 integration — Make it fly on real hardware Hardware testing guides — Calibration, deployment 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: VINS-Mono Paper: Qin et al., "A Robust and Versatile Monocular Visual-Inertial State Estimator" (IEEE T-RO, 2018) ORB-SLAM3: Campos et al., "An Accurate Open-Source SLAM System" (IEEE T-RO, 2021) Nav2 Documentation: navigation.ros.org 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.

Read full story →