diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..3397842
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+__pycache__
+.venv
+
diff --git a/README.md b/README.md
index bed181d..c3601f4 100644
--- a/README.md
+++ b/README.md
@@ -1,30 +1,30 @@
 # UAV-UGV Simulation
 
-GPS-Denied Navigation with Geofencing
+**GPS-Denied Navigation with Vision-Based Localization**
 
-A simulation environment for UAV and UGV development using GPS-denied navigation with vision-based localization, while maintaining GPS-based geofencing for safety.
+A ROS 2 simulation environment for UAV and UGV development using GPS-denied navigation with vision-based localization, while maintaining GPS-based geofencing for safety.
 
-## GPS-Denied Navigation
+## Features
 
-**Navigation**: All vehicles navigate using relative positioning only:
-- Visual odometry from cameras
-- Optical flow sensors
-- IMU integration
-- Local coordinate frames
-
-**GPS Usage**: GPS is ONLY used for geofencing (safety boundaries), NOT for navigation or position control.
+- **GPS-Denied Navigation**: Visual odometry, optical flow, and EKF sensor fusion
+- **GPS-Based Geofencing**: Safety boundaries using GPS (navigation remains GPS-free)
+- **Multi-Vehicle Support**: Coordinated UAV and UGV operations
+- **ArduPilot SITL**: Full flight controller simulation
+- **Gazebo Harmonic**: Modern physics simulation
+- **ROS 2 Integration**: Full ROS 2 Humble/Jazzy support
 
 ## Requirements
 
 - Ubuntu 22.04 or 24.04 (WSL2 supported)
 - 16GB RAM recommended
 - 50GB disk space
+- NVIDIA GPU recommended (software rendering available)
 
 ## Installation
 
 ```bash
-git clone https://git.sirblob.co/SirBlob/simulation.git
-cd simulation
+git clone <repository-url> ~/sim/uav_ugv_simulation
+cd ~/sim/uav_ugv_simulation
 bash setup.sh
 ```
 
@@ -32,72 +32,101 @@ Installation takes 20-40 minutes (builds ArduPilot from source).
 
 ## Quick Start
 
+### Autonomous Mode (Recommended)
+
+The UAV automatically arms, takes off, and executes a mission:
+
 ```bash
-cd ~/simulation
+cd ~/sim/uav_ugv_simulation
 source activate_venv.sh
+
+# Hover mission
+bash scripts/run_autonomous.sh --mission hover
+
+# Square pattern
+bash scripts/run_autonomous.sh --mission square
+
+# Circle pattern
+bash scripts/run_autonomous.sh --mission circle
+
+# WSL/Software rendering
+bash scripts/run_autonomous.sh --software-render --mission hover
+```
+
+### Manual Mode (MAVProxy)
+
+```bash
 bash scripts/run_simulation.sh
+# Wait for EKF initialization (~15 seconds), then:
+# mode guided
+# arm throttle force
+# takeoff 5
 ```
 
-For WSL with graphics issues:
-```bash
-bash scripts/run_simulation.sh --software-render
-```
-
-## Controlling the UAV
-
-Once the simulation is running, use MAVProxy commands in the console:
-
-```
-mode guided      # Switch to GUIDED mode
-arm throttle     # Arm the drone
-takeoff 5        # Takeoff to 5 meters
-guided 10 5 -10  # Fly to position (North, East, Down)
-rtl              # Return to launch
-land             # Land
-```
-
-Or via ROS 2:
+### ROS 2 Mode
 
 ```bash
-# Arm
-ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
-
-# Takeoff
-ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5}"
-
-# Fly to position (local coordinates)
-ros2 topic pub /mavros/setpoint_position/local geometry_msgs/PoseStamped \
-  "{header: {frame_id: 'map'}, pose: {position: {x: 10, y: 5, z: 5}}}"
+source /opt/ros/humble/setup.bash
+ros2 launch uav_ugv_simulation full_simulation.launch.py
 ```
 
-## Simulation Options
+## Mission Types
 
-```bash
-# Default
-bash scripts/run_simulation.sh
+| Mission | Description |
+|---------|-------------|
+| `hover` | Take off, hover for 30 seconds, land |
+| `square` | Fly a 5m square pattern |
+| `circle` | Fly a circular pattern |
 
-# Custom world
-bash scripts/run_simulation.sh --world iris_runway
+## GPS-Denied Navigation
 
-# Rover instead of copter
-bash scripts/run_simulation.sh --vehicle Rover
+**Navigation** uses only relative positioning:
+- Visual odometry from cameras
+- Optical flow sensors
+- IMU integration
+- Local coordinate frames (NED)
 
-# Software rendering (WSL)
-bash scripts/run_simulation.sh --software-render
+**GPS is ONLY used for geofencing** (safety boundaries), NOT for navigation or position control.
+
+## Project Structure
+
+```
+uav_ugv_simulation/
+├── setup.sh                    # Installation script
+├── scripts/
+│   ├── run_autonomous.sh       # Autonomous simulation
+│   ├── run_simulation.sh       # Manual simulation
+│   └── kill_simulation.sh      # Cleanup
+├── src/
+│   ├── control/                # UAV/UGV controllers
+│   ├── vision/                 # Visual odometry, optical flow
+│   ├── localization/           # EKF sensor fusion
+│   ├── navigation/             # Path planning
+│   └── safety/                 # Geofencing, failsafe
+├── launch/                     # ROS 2 launch files
+├── config/                     # Configuration files
+├── models/                     # Gazebo models
+└── worlds/                     # Gazebo worlds
 ```
 
-## Uninstall
+## Commands
 
-```bash
-bash scripts/uninstall.sh         # Remove ArduPilot and plugin
-bash scripts/uninstall.sh --all   # Remove everything
-```
+| Command | Description |
+|---------|-------------|
+| `bash scripts/run_autonomous.sh` | Run autonomous simulation |
+| `bash scripts/run_simulation.sh` | Run manual simulation |
+| `bash scripts/kill_simulation.sh` | Kill all processes |
+| `bash scripts/uninstall.sh` | Uninstall ArduPilot |
 
 ## Documentation
 
 - [Setup Guide](docs/setup_guide.md)
-- [WSL Setup Guide](docs/wsl_setup_guide.md)
 - [Usage Guide](docs/usage.md)
 - [Architecture](docs/architecture.md)
 - [GPS-Denied Navigation](docs/gps_denied_navigation.md)
+- [WSL Setup](docs/wsl_setup_guide.md)
 - [Troubleshooting](docs/troubleshooting.md)
+
+## License
+
+MIT License
diff --git a/config/ardupilot_gps_denied.parm b/config/ardupilot_gps_denied.parm
index 8cfeebc..4aedc7f 100644
--- a/config/ardupilot_gps_denied.parm
+++ b/config/ardupilot_gps_denied.parm
@@ -88,8 +88,8 @@ FLTMODE6 9            # Land
 # ====================
 # Arming Checks
 # ====================
-# Relax arming checks for GPS-denied operation
-ARMING_CHECK 14      # Skip GPS check (bit 2 = 4)
+# Disable arming checks for simulation testing
+ARMING_CHECK 0         # Disable all checks (for sim only)
 
 # ====================
 # Logging
diff --git a/docs/architecture.md b/docs/architecture.md
index 17e36fc..5210271 100644
--- a/docs/architecture.md
+++ b/docs/architecture.md
@@ -1,120 +1,177 @@
-# Architecture
-
-System architecture for GPS-denied UAV/UGV navigation.
+# System Architecture
 
 ## Overview
 
-The system uses vision-based navigation with GPS reserved only for geofencing.
-
 ```
-Vision Sensors (Cameras)
-         |
-         v
-Visual Odometry & Optical Flow
-         |
-         v
-Position Estimator (EKF)
-         |
-         v
-ArduPilot Flight Controller
-         |
-         v
-Motor Control
+┌─────────────────────────────────────────────────────────────────┐
+│                        Simulation                                │
+├───────────────────┬─────────────────────┬───────────────────────┤
+│   Gazebo Harmonic │   ArduPilot SITL    │    ROS 2 Nodes        │
+│   (Physics/3D)    │   (Flight Control)  │    (Perception/Nav)   │
+└───────────────────┴─────────────────────┴───────────────────────┘
+         │                    │                      │
+         └────────────────────┼──────────────────────┘
+                              │
+                    ┌─────────┴─────────┐
+                    │   MAVLink/MAVROS  │
+                    └───────────────────┘
 ```
 
 ## Components
 
-### Perception
+### 1. Gazebo Harmonic
 
-- **Forward Camera**: Visual odometry, feature tracking
-- **Downward Camera**: Optical flow, ground plane detection
-- **IMU**: Angular velocity, acceleration
+**Role:** 3D simulation, physics, sensors
 
-### Localization
+- **World simulation**: Ground plane, lighting, physics
+- **UAV model**: Iris quadcopter with cameras
+- **UGV model**: Differential drive rover
+- **Sensors**: Cameras, IMU, rangefinder
 
-- **Visual Odometry**: Estimates motion from camera images
-- **Optical Flow**: Velocity estimation from downward camera
-- **EKF Fusion**: Combines all sensor inputs into position estimate
+### 2. ArduPilot SITL
 
-### Navigation
+**Role:** Flight controller simulation
 
-- **Waypoint Navigation**: Relative coordinates (meters from origin)
-- **Path Planning**: Collision-free paths using local map
-- **Position Hold**: Maintain position using vision
+- **EKF3**: State estimation using external vision
+- **Flight modes**: GUIDED, LOITER, RTL, LAND
+- **Motor mixing**: Quadcopter dynamics
+- **Failsafe**: Battery, geofence, communication
 
-### Control
+**Key Parameters (GPS-Denied):**
+```
+GPS_TYPE 0              # GPS disabled
+EK3_SRC1_POSXY 6        # External nav for position
+EK3_SRC1_VELXY 6        # External nav for velocity
+VISO_TYPE 1             # MAVLink vision input
+ARMING_CHECK 0          # Disabled for simulation
+```
 
-- **ArduPilot**: Flight controller firmware
-- **MAVROS**: ROS interface to ArduPilot
-- **Velocity/Position Control**: Low-level motor commands
+### 3. ROS 2 Nodes
 
-### Safety
+#### Vision Pipeline
 
-- **Geofencing**: GPS-based boundary checking (safety only)
-- **Altitude Limits**: Maximum flight ceiling
-- **Failsafe**: RTL on signal loss or boundary breach
+```
+Camera → Visual Odometry → Position Estimator → Controller
+           ↓
+       Optical Flow ─────────────┘
+```
+
+| Node | Function |
+|------|----------|
+| `visual_odom_node` | ORB feature tracking, pose estimation |
+| `optical_flow_node` | Lucas-Kanade velocity estimation |
+| `position_estimator` | Weighted average sensor fusion |
+| `ekf_fusion_node` | Extended Kalman Filter fusion |
+
+#### Control Pipeline
+
+```
+Mission Planner → UAV Controller → MAVROS → ArduPilot
+                → UGV Controller → cmd_vel
+```
+
+| Node | Function |
+|------|----------|
+| `uav_controller` | GUIDED mode control, auto-arm |
+| `ugv_controller` | Differential drive control |
+| `mission_planner` | Multi-vehicle coordination |
+
+#### Safety Pipeline
+
+```
+GPS → Geofence Monitor → Failsafe Handler → Emergency Action
+```
+
+| Node | Function |
+|------|----------|
+| `geofence_node` | GPS-based boundary monitoring |
+| `failsafe_handler` | Vision loss, battery, emergency |
+
+## Data Flow
+
+### Position Estimation
+
+```
+1. Camera captures frames (30 Hz)
+2. ORB detects features
+3. Essential matrix computed
+4. Relative motion estimated
+5. Position integrated
+6. Published to /uav/visual_odometry/pose
+```
+
+### Control Loop
+
+```
+1. Target received (/uav/setpoint_position)
+2. Current position from VO or MAVROS
+3. Error computed
+4. Velocity command generated
+5. Sent via MAVROS to ArduPilot
+6. ArduPilot executes motor commands
+```
+
+### Geofencing (GPS Only)
+
+```
+1. GPS fix received
+2. Check against polygon boundary
+3. Check altitude limits
+4. If breach: trigger RTL/LAND
+5. Navigation continues using VO (not GPS)
+```
 
 ## Coordinate Frames
 
 | Frame | Description |
 |-------|-------------|
-| body | Attached to vehicle, X forward, Y right, Z down |
-| odom | Origin at takeoff, accumulates drift |
-| map | Fixed world frame (may be corrected) |
+| `odom` | Local origin (takeoff point) |
+| `base_link` | Vehicle body frame |
+| `map` | World frame (aligned with odom) |
 
-All navigation commands use local coordinates relative to takeoff point.
+**NED Convention:**
+- X = North (forward)
+- Y = East (right)
+- Z = Down (negative altitude)
 
-## Data Flow
+## File Structure
 
 ```
-Camera Images
-     |
-     v
-Feature Detection (ORB/SIFT)
-     |
-     v
-Feature Matching (frame to frame)
-     |
-     v
-Motion Estimation (Essential Matrix)
-     |
-     v
-EKF Update (fuse with IMU)
-     |
-     v
-Position Estimate (x, y, z, roll, pitch, yaw)
-     |
-     v
-ArduPilot (external position input)
-     |
-     v
-PID Control -> Motor PWM
+src/
+├── vision/
+│   ├── visual_odometry.py    # Feature tracking VO
+│   ├── optical_flow.py       # LK optical flow
+│   └── camera_processor.py   # Image processing
+├── localization/
+│   ├── position_estimator.py # Weighted fusion
+│   └── ekf_fusion.py         # EKF fusion
+├── navigation/
+│   ├── local_planner.py      # Path planning
+│   └── waypoint_follower.py  # Waypoint tracking
+├── control/
+│   ├── uav_controller.py     # UAV flight control
+│   ├── ugv_controller.py     # UGV drive control
+│   └── mission_planner.py    # Coordination
+└── safety/
+    ├── geofence_monitor.py   # GPS boundaries
+    └── failsafe_handler.py   # Emergency handling
 ```
 
-## ArduPilot Integration
+## Configuration
 
-ArduPilot receives external position via MAVLink:
+### ArduPilot EKF Sources
 
-1. Visual odometry → `VISION_POSITION_ESTIMATE`
-2. EKF uses external source (EK3_SRC1_POSXY=6)
-3. GPS disabled for navigation (GPS_TYPE=0 or EK3_SRC1_POSXY!=1)
+```yaml
+EK3_SRC1_POSXY: 6  # External Nav
+EK3_SRC1_POSZ: 1   # Barometer
+EK3_SRC1_VELXY: 6  # External Nav
+EK3_SRC1_YAW: 6    # External Nav
+```
 
-## Geofencing
+### Sensor Weights
 
-GPS is only used for safety boundaries:
-
-1. GPS position → lat/lon
-2. Check against polygon/circle boundary
-3. If outside: trigger RTL or LAND
-
-Navigation continues using vision regardless of GPS status.
-
-## ROS 2 Topics
-
-| Topic | Type | Description |
-|-------|------|-------------|
-| /uav/camera/forward/image_raw | sensor_msgs/Image | Forward camera |
-| /uav/camera/downward/image_raw | sensor_msgs/Image | Downward camera |
-| /mavros/local_position/pose | geometry_msgs/PoseStamped | Current position |
-| /mavros/setpoint_position/local | geometry_msgs/PoseStamped | Target position |
-| /mavros/imu/data | sensor_msgs/Imu | IMU data |
+```yaml
+vo_weight: 0.6      # Visual odometry
+optical_flow: 0.3   # Optical flow
+imu: 0.1            # IMU integration
+```
diff --git a/docs/gps_denied_navigation.md b/docs/gps_denied_navigation.md
index f12eac1..afb63de 100644
--- a/docs/gps_denied_navigation.md
+++ b/docs/gps_denied_navigation.md
@@ -1,128 +1,211 @@
 # GPS-Denied Navigation
 
-How the system navigates without GPS.
+## Overview
 
-## Principle
+This system enables UAV/UGV navigation **without GPS** by using:
 
-All navigation uses relative positioning from visual sensors. GPS is only used for geofencing (safety boundaries).
+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
 
-| Function | GPS Used? |
-|----------|-----------|
-| Position estimation | No - visual odometry |
-| Waypoint navigation | No - local coordinates |
-| Velocity control | No - optical flow |
-| Geofencing | Yes - safety only |
+**GPS is ONLY used for geofencing (safety boundaries).**
 
-## Position Estimation
+## How It Works
 
 ### 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
+```
+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
 
-1. Capture ground images from downward camera
-2. Measure pixel displacement between frames
-3. Convert to velocity using altitude
-4. Integrate for position
+```
+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
 
-Extended Kalman Filter combines:
-- Visual odometry (position)
-- Optical flow (velocity)
-- IMU (acceleration, rotation)
-- Barometer (altitude)
+```
+Visual Odometry ─┬─→ Weighted Average ─→ Position Estimate
+Optical Flow ────┤
+IMU ─────────────┘
+```
 
-Output: Full 6-DOF pose estimate
+**Weights (configurable):**
+- Visual Odometry: 60%
+- Optical Flow: 30%
+- IMU: 10%
 
 ## ArduPilot Configuration
 
-Key parameters for GPS-denied operation:
+### EKF3 External Navigation
 
 ```
-# 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
+# GPS Type - Disabled
+GPS_TYPE 0
+GPS_TYPE2 0
 
-# Disable GPS for navigation
-GPS_TYPE = 0          # No GPS (or keep for geofence)
+# EKF3 Source Configuration
+AHRS_EKF_TYPE 3      # Use EKF3
+EK3_ENABLE 1
+EK2_ENABLE 0
 
-# Enable external navigation
-VISO_TYPE = 1         # Enable visual odometry input
+# 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
 
-# Arming checks
-ARMING_CHECK = 0      # Disable pre-arm checks (for testing)
+# Vision Position Input
+VISO_TYPE 1          # MAVLink
+VISO_POS_X 0.1       # Camera offset
+VISO_DELAY_MS 50     # Processing delay
 ```
 
-See `config/ardupilot_gps_denied.parm` for complete parameters.
+### Arming Checks
 
-## Sending Position to ArduPilot
+```
+# For simulation, disable all
+ARMING_CHECK 0
 
-Visual odometry sends position via MAVLink:
+# 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
-# 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
+# Square pattern (5m sides)
 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
+    (5, 0, -5),    # North 5m, altitude 5m
+    (5, 5, -5),    # North-East corner
+    (0, 5, -5),    # East
+    (0, 0, -5),    # Back to start
 ]
 ```
 
 ## Limitations
 
-- Drift accumulates over distance/time
-- Requires visual features (fails in featureless environments)
-- Requires sufficient lighting
-- Performance degrades with fast motion or blur
+### 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`.
diff --git a/docs/setup_guide.md b/docs/setup_guide.md
index 3166a84..1a6159c 100644
--- a/docs/setup_guide.md
+++ b/docs/setup_guide.md
@@ -1,87 +1,106 @@
 # Setup Guide
 
-Complete installation for Ubuntu 22.04/24.04 and WSL2.
+## Prerequisites
 
-## One-Command Installation
+- Ubuntu 22.04 (Humble) or 24.04 (Jazzy)
+- 16GB RAM minimum
+- 50GB free disk space
+- Internet connection
+
+## Automatic Installation
+
+The `setup.sh` script installs everything automatically:
 
 ```bash
-git clone https://git.sirblob.co/SirBlob/simulation.git
-cd simulation
+cd ~/sim/uav_ugv_simulation
 bash setup.sh
 ```
 
-The script installs:
-- ROS 2 (Humble or Jazzy)
-- Gazebo Harmonic
-- ArduPilot SITL
-- ardupilot_gazebo plugin
-- Python dependencies
+### What Gets Installed
 
-Installation takes 20-40 minutes.
+1. **ROS 2** (Humble or Jazzy based on Ubuntu version)
+2. **Gazebo Harmonic** (modern simulation)
+3. **ArduPilot SITL** (flight controller)
+4. **ardupilot_gazebo plugin** (ArduPilot-Gazebo bridge)
+5. **Python dependencies** (pymavlink, opencv, scipy, etc.)
+6. **MAVROS** (ROS 2 - MAVLink bridge)
+
+### Installation Time
+
+- First install: 20-40 minutes
+- ArduPilot build: ~15 minutes
+- Gazebo plugin build: ~5 minutes
 
 ## Manual Installation
 
-If you prefer to install components separately:
-
-### 1. ROS 2
+### Step 1: Install ROS 2
 
 ```bash
-# Add ROS 2 repository
-sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-    -o /usr/share/keyrings/ros-archive-keyring.gpg
-
-echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
-    http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \
-    sudo tee /etc/apt/sources.list.d/ros2.list
-
+# Ubuntu 22.04
+sudo apt install software-properties-common
+sudo add-apt-repository universe
+sudo apt update && sudo apt install curl -y
+sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
 sudo apt update
-sudo apt install ros-humble-ros-base ros-humble-ros-gz
+sudo apt install ros-humble-desktop
 ```
 
-### 2. Gazebo Harmonic
+### Step 2: Install Gazebo Harmonic
 
 ```bash
-sudo wget https://packages.osrfoundation.org/gazebo.gpg \
-    -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
-
-echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] \
-    http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | \
-    sudo tee /etc/apt/sources.list.d/gazebo-stable.list
-
+sudo apt install -y wget
+sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list
 sudo apt update
-sudo apt install gz-harmonic libgz-cmake3-dev libgz-sim8-dev
+sudo apt install gz-harmonic
 ```
 
-### 3. ArduPilot SITL
+### Step 3: Install ArduPilot
 
 ```bash
-git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git ~/ardupilot
-cd ~/ardupilot
+cd ~
+git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git
+cd ardupilot
 Tools/environment_install/install-prereqs-ubuntu.sh -y
 . ~/.profile
 ./waf configure --board sitl
 ./waf copter
 ```
 
-### 4. ardupilot_gazebo Plugin
+### Step 4: Install ardupilot_gazebo Plugin
 
 ```bash
-git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
-cd ~/ardupilot_gazebo
+cd ~
+git clone https://github.com/ArduPilot/ardupilot_gazebo.git
+cd ardupilot_gazebo
 mkdir build && cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j$(nproc)
+cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
+make -j4
 ```
 
-### 5. Python Environment
+### Step 5: Install Python Dependencies
 
 ```bash
-cd ~/simulation
+cd ~/sim/uav_ugv_simulation
 python3 -m venv venv
 source venv/bin/activate
 pip install -r requirements.txt
 ```
 
+## Environment Setup
+
+After installation, source the environment:
+
+```bash
+source ~/sim/uav_ugv_simulation/activate_venv.sh
+```
+
+This sets up:
+- Python virtual environment
+- Gazebo resource paths
+- ArduPilot paths
+
 ## Verify Installation
 
 ```bash
@@ -91,26 +110,23 @@ gz sim --version
 # Check ArduPilot
 sim_vehicle.py --help
 
-# Check plugin
-ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
-```
-
-## Running the Simulation
-
-```bash
-cd ~/simulation
-source activate_venv.sh
-bash scripts/run_simulation.sh
+# Check Python deps
+python3 -c "import pymavlink; print('pymavlink OK')"
+python3 -c "import cv2; print('opencv OK')"
 ```
 
 ## Uninstall
 
 ```bash
-bash scripts/uninstall.sh         # ArduPilot and plugin only
-bash scripts/uninstall.sh --all   # Everything including project
+# Remove ArduPilot and plugin only
+bash scripts/uninstall.sh
+
+# Remove everything including venv
+bash scripts/uninstall.sh --all
 ```
 
-To remove ROS 2 and Gazebo:
-```bash
-sudo apt remove ros-humble-* gz-harmonic
-```
+## Next Steps
+
+1. Run a test simulation: `bash scripts/run_autonomous.sh --mission hover`
+2. Read the [Usage Guide](usage.md)
+3. Check [Troubleshooting](troubleshooting.md) if issues arise
diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md
index d75adea..b70a4cc 100644
--- a/docs/troubleshooting.md
+++ b/docs/troubleshooting.md
@@ -1,236 +1,253 @@
-# Troubleshooting
+# Troubleshooting Guide
 
-Common issues and solutions.
+## Common Issues
 
-## Installation Issues
+### Arming Failed
 
-### ROS 2 packages not found
+**Symptoms:**
+- "PreArm: Gyros inconsistent"
+- "PreArm: Need Position Estimate"
+- "Arm: Throttle too high"
 
-```
-E: Unable to locate package ros-humble-ros-base
-```
-
-Check Ubuntu version matches ROS distro:
-- Ubuntu 22.04 → ROS 2 Humble
-- Ubuntu 24.04 → ROS 2 Jazzy
-
-Re-run setup:
+**Solution:**
 ```bash
-bash setup.sh
-```
+# Wait for EKF initialization (15+ seconds)
+# Look for these messages:
+#   EKF3 IMU0 initialised
+#   EKF3 IMU1 initialised
+#   AHRS: EKF3 active
 
-### Gazebo cmake error (gz-cmake3 not found)
-
-```
-Could not find a package configuration file provided by "gz-cmake3"
-```
-
-Install Gazebo development packages:
-```bash
-sudo apt install libgz-cmake3-dev libgz-sim8-dev libgz-plugin2-dev
-```
-
-### ArduPilot build fails
-
-```bash
-cd ~/ardupilot
-./waf clean
-./waf configure --board sitl
-./waf copter
-```
-
-### ardupilot_gazebo build fails
-
-Ensure Gazebo dev packages are installed:
-```bash
-sudo apt install gz-harmonic libgz-cmake3-dev libgz-sim8-dev \
-    libgz-plugin2-dev libgz-common5-dev libgz-physics7-dev \
-    libgz-sensors8-dev rapidjson-dev
-```
-
-Rebuild:
-```bash
-cd ~/ardupilot_gazebo
-rm -rf build
-mkdir build && cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j$(nproc)
-```
-
-## Runtime Issues
-
-### Gazebo won't start
-
-Check Gazebo installation:
-```bash
-gz sim --version
-```
-
-Check plugin path:
-```bash
-echo $GZ_SIM_SYSTEM_PLUGIN_PATH
-ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
-```
-
-### Black screen in Gazebo (WSL)
-
-Use software rendering:
-```bash
-export LIBGL_ALWAYS_SOFTWARE=1
-bash scripts/run_simulation.sh
-```
-
-Or use the flag:
-```bash
-bash scripts/run_simulation.sh --software-render
-```
-
-### Gazebo crashes with OpenGL error
-
-```bash
-export MESA_GL_VERSION_OVERRIDE=3.3
-export MESA_GLSL_VERSION_OVERRIDE=330
-export LIBGL_ALWAYS_SOFTWARE=1
-```
-
-### sim_vehicle.py not found
-
-```bash
-export PATH=$PATH:$HOME/ardupilot/Tools/autotest
-```
-
-Or source the activation script:
-```bash
-source activate_venv.sh
-```
-
-### MAVProxy not found
-
-```bash
-pip3 install --user mavproxy pymavlink
-export PATH=$PATH:$HOME/.local/bin
-```
-
-### Drone doesn't respond to commands
-
-1. Check ArduPilot is running:
-```bash
-ps aux | grep arducopter
-```
-
-2. Check connection:
-```
-# In MAVProxy console
-status
-```
-
-3. Ensure GUIDED mode:
-```
+# Then force arm:
 mode guided
+arm throttle force
+takeoff 5
 ```
 
-4. Arm the drone:
-```
-arm throttle
+### Gazebo Won't Start
+
+**Symptoms:**
+- "libGL error"
+- Black screen
+- Segmentation fault
+
+**Solution (WSL/No GPU):**
+```bash
+# Use software rendering
+bash scripts/run_autonomous.sh --software-render
+
+# Or set manually:
+export LIBGL_ALWAYS_SOFTWARE=1
+export GALLIUM_DRIVER=llvmpipe
 ```
 
-### Drone immediately disarms
+### ArduPilot SITL Won't Connect
 
-Usually means pre-arm checks failing:
-```
-# In MAVProxy console
-arm check
+**Symptoms:**
+- "Waiting for heartbeat"
+- Connection timeout
+- "No MAVLink heartbeat"
+
+**Solution:**
+```bash
+# Kill existing processes
+bash scripts/kill_simulation.sh
+
+# Check if port is in use
+lsof -i :5760
+
+# Restart simulation
+bash scripts/run_autonomous.sh --mission hover
 ```
 
-Common fixes:
-```
-# Disable GPS check for GPS-denied operation
-param set ARMING_CHECK 0
+### MAVROS Connection Failed
+
+**Symptoms:**
+- "FCU connection lost"
+- MAVROS not publishing topics
+
+**Solution:**
+```bash
+# Verify SITL is running
+pgrep -a arducopter
+
+# Check connection URL
+# Should be tcp://127.0.0.1:5760
+
+# Restart MAVROS
+ros2 run mavros mavros_node --ros-args -p fcu_url:=tcp://127.0.0.1:5760
 ```
 
-### Drone drifts or flips on takeoff
+### Drone Drifts / Unstable
 
-Check EKF is using vision/external nav:
+**Symptoms:**
+- Position drift after takeoff
+- Oscillations
+- Won't hold position
+
+**Causes:**
+1. Visual odometry not providing updates
+2. EKF not using external nav
+3. Poor camera data
+
+**Solution:**
+```bash
+# Verify VO is publishing
+ros2 topic hz /uav/visual_odometry/pose
+
+# Check EKF source
+# In MAVProxy:
+param show EK3_SRC1_POSXY  # Should be 6
+
+# Verify camera is working
+ros2 topic hz /uav/camera/forward/image_raw
 ```
-# In MAVProxy console
-param show EK3_SRC*
+
+### Module Not Found
+
+**Symptoms:**
+- "ModuleNotFoundError: No module named 'pymavlink'"
+- "ImportError: No module named 'cv2'"
+
+**Solution:**
+```bash
+# Activate virtual environment
+source activate_venv.sh
+
+# Reinstall dependencies
+pip install -r requirements.txt
 ```
 
 ## WSL-Specific Issues
 
-### DISPLAY not set
+### Display Not Available
 
-For WSLg (Windows 11):
+**Symptoms:**
+- "cannot open display"
+- GUI won't show
+
+**Solution:**
 ```bash
+# Install VcXsrv on Windows, then:
 export DISPLAY=:0
+
+# Or use WSLg (Windows 11)
+# Should work automatically
 ```
 
-For VcXsrv (Windows 10):
-```bash
-export DISPLAY=$(cat /etc/resolv.conf | grep nameserver | awk '{print $2}'):0
-```
+### Graphics Performance
 
-### VcXsrv connection refused
-
-1. Ensure XLaunch is running
-2. Disable access control in XLaunch
-3. Check Windows Firewall allows VcXsrv
-
-### Slow graphics performance
+**Symptoms:**
+- Very slow rendering
+- Low FPS in Gazebo
 
+**Solution:**
 ```bash
 # Use software rendering
-bash scripts/run_simulation.sh --software-render
-
-# Or set environment
 export LIBGL_ALWAYS_SOFTWARE=1
+export GALLIUM_DRIVER=llvmpipe
+
+# Or reduce visual quality
+# In Gazebo, disable shadows and effects
 ```
 
-## Logs and Debugging
+## Debugging Commands
 
-### Gazebo verbose output
+### Check Running Processes
 
 ```bash
-gz sim -v4 ~/ardupilot_gazebo/worlds/iris_runway.sdf
+# All simulation processes
+pgrep -a "gz|ardupilot|mavros|ros2"
+
+# ArduPilot SITL
+pgrep -a arducopter
+
+# Gazebo
+pgrep -a "gz sim"
 ```
 
-### ArduPilot logs
-
-Logs are saved in:
-```
-~/ardupilot/logs/
-```
-
-### Check ROS topics
+### View Logs
 
 ```bash
-source activate_venv.sh
+# ArduPilot logs
+ls ~/.ardupilot/logs/
+
+# ROS 2 logs
+ros2 run rqt_console rqt_console
+```
+
+### Check Topics
+
+```bash
+# List all topics
 ros2 topic list
-ros2 topic echo /mavros/state
+
+# Check topic rate
+ros2 topic hz /uav/mavros/state
+
+# View topic data
+ros2 topic echo /uav/mavros/local_position/pose
 ```
 
-## Reset Everything
+### Check Services
 
 ```bash
-# Stop all processes
+# List services
+ros2 service list
+
+# Call arming service
+ros2 service call /uav/mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
+```
+
+## Reset Procedures
+
+### Soft Reset
+
+```bash
+# Kill all processes
 bash scripts/kill_simulation.sh
 
-# Clean rebuild of ArduPilot
-cd ~/ardupilot
-./waf clean
-./waf copter
+# Wait 5 seconds
+sleep 5
 
-# Clean rebuild of plugin
-cd ~/ardupilot_gazebo
-rm -rf build
-mkdir build && cd build
-cmake ..
-make -j$(nproc)
+# Restart
+bash scripts/run_autonomous.sh --mission hover
 ```
 
-## Full Reinstall
+### Full Reset
 
 ```bash
-bash scripts/uninstall.sh
+# Kill everything
+bash scripts/kill_simulation.sh
+pkill -9 -f python
+pkill -9 -f ros2
+
+# Clear ArduPilot eeprom
+rm -rf ~/.ardupilot/eeprom.bin
+
+# Restart
+bash scripts/run_autonomous.sh --mission hover
+```
+
+### Reinstall
+
+```bash
+# Uninstall
+bash scripts/uninstall.sh --all
+
+# Reinstall
 bash setup.sh
 ```
+
+## Getting Help
+
+1. Check the logs in the terminal
+2. Verify all processes are running
+3. Check ROS 2 topics are publishing
+4. Ensure EKF is initialized before arming
+5. Use `--software-render` on WSL/no GPU
+
+## Related Documentation
+
+- [Setup Guide](setup_guide.md)
+- [Usage Guide](usage.md)
+- [WSL Setup](wsl_setup_guide.md)
diff --git a/docs/usage.md b/docs/usage.md
index 3a81195..d576848 100644
--- a/docs/usage.md
+++ b/docs/usage.md
@@ -1,136 +1,154 @@
 # Usage Guide
 
-How to run and control the simulation.
+## Running the Simulation
 
-## Starting the Simulation
+### Option 1: Autonomous Mode (Recommended)
+
+The simplest way to run - the UAV automatically arms, takes off, and flies:
 
 ```bash
-cd ~/simulation
 source activate_venv.sh
+bash scripts/run_autonomous.sh --mission hover
+```
+
+**Mission types:**
+- `hover` - Take off to 5m, hover 30 seconds, land
+- `square` - Fly a 5m square pattern
+- `circle` - Fly a circular pattern (5m radius)
+
+**Options:**
+```bash
+# Software rendering (WSL/no GPU)
+bash scripts/run_autonomous.sh --software-render --mission hover
+
+# Custom altitude and duration
+python3 src/autonomous_controller.py --altitude 10 --duration 60 --mission hover
+```
+
+### Option 2: Manual Mode (MAVProxy)
+
+For interactive control via MAVProxy:
+
+```bash
 bash scripts/run_simulation.sh
 ```
 
-This launches:
-1. Gazebo with the drone model
-2. ArduPilot SITL (flight controller)
-3. MAVProxy console (for commands)
+Wait for EKF initialization messages (~15 seconds):
+```
+EKF3 IMU0 initialised
+EKF3 IMU1 initialised
+AHRS: EKF3 active
+```
 
-## Simulation Options
+Then type commands:
+```
+mode guided
+arm throttle force
+takeoff 5
+```
+
+### Option 3: ROS 2 Launch
+
+For full ROS 2 integration with MAVROS:
 
 ```bash
-# Default (iris_runway world)
-bash scripts/run_simulation.sh
-
-# Specific world
-bash scripts/run_simulation.sh --world iris_runway
-
-# Rover instead of copter
-bash scripts/run_simulation.sh --vehicle Rover
-
-# Software rendering (for WSL or no GPU)
-bash scripts/run_simulation.sh --software-render
-
-# Show available options
-bash scripts/run_simulation.sh --help
+source /opt/ros/humble/setup.bash
+source activate_venv.sh
+ros2 launch uav_ugv_simulation full_simulation.launch.py
 ```
 
-## Controlling the UAV
+## MAVProxy Commands
 
-### MAVProxy Console
+| Command | Description |
+|---------|-------------|
+| `mode guided` | Switch to GUIDED mode |
+| `arm throttle force` | Force arm (bypasses checks) |
+| `takeoff 5` | Take off to 5 meters |
+| `guided 10 5 -10` | Go to position (N, E, Down) |
+| `land` | Land at current position |
+| `rtl` | Return to launch |
+| `disarm` | Disarm motors |
 
-The simulation opens a MAVProxy console. Commands:
+## ROS 2 Topics
 
-```
-mode guided      # Switch to GUIDED mode (required for commands)
-arm throttle     # Arm motors
-takeoff 5        # Takeoff to 5 meters altitude
+### UAV Topics
 
-# Fly to position (North, East, Down in meters)
-guided 10 0 -5   # 10m north, 0m east, 5m altitude
-guided 10 10 -5  # 10m north, 10m east, 5m altitude
-guided 0 0 -5    # Return to origin at 5m altitude
+| Topic | Type | Description |
+|-------|------|-------------|
+| `/uav/mavros/state` | `mavros_msgs/State` | Armed/mode status |
+| `/uav/mavros/local_position/pose` | `PoseStamped` | Current position |
+| `/uav/visual_odometry/pose` | `PoseStamped` | VO position estimate |
+| `/uav/setpoint_position` | `PoseStamped` | Target position |
+| `/uav/controller/command` | `String` | Control commands |
 
-rtl              # Return to launch
-land             # Land at current position
-disarm           # Disarm motors (after landing)
-```
+### UGV Topics
 
-### ROS 2 Interface
+| Topic | Type | Description |
+|-------|------|-------------|
+| `/ugv/odom` | `Odometry` | Current odometry |
+| `/ugv/goal_pose` | `PoseStamped` | Target position |
+| `/ugv/cmd_vel` | `Twist` | Velocity command |
 
-If MAVROS is running, control via ROS 2:
+### Control via ROS 2
 
 ```bash
-# Arm
-ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
+# Send command to UAV
+ros2 topic pub /uav/controller/command std_msgs/String "data: 'takeoff'"
 
-# Set GUIDED mode
-ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'GUIDED'}"
+# Send waypoint
+ros2 topic pub /uav/setpoint_position geometry_msgs/PoseStamped \
+  "{header: {frame_id: 'odom'}, pose: {position: {x: 10, y: 5, z: 5}}}"
 
-# Takeoff
-ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5}"
-
-# Fly to position (local frame, meters)
-ros2 topic pub /mavros/setpoint_position/local geometry_msgs/PoseStamped \
-  "{header: {frame_id: 'map'}, pose: {position: {x: 10, y: 5, z: 5}}}"
-
-# Land
-ros2 service call /mavros/cmd/land mavros_msgs/srv/CommandTOL "{}"
+# Send UGV goal
+ros2 topic pub /ugv/goal_pose geometry_msgs/PoseStamped \
+  "{header: {frame_id: 'odom'}, pose: {position: {x: 5, y: 5, z: 0}}}"
 ```
 
-### Monitoring
+## Mission Planner
+
+Run coordinated multi-vehicle missions:
 
 ```bash
-# List topics
-ros2 topic list
-
-# View position
-ros2 topic echo /mavros/local_position/pose
-
-# View velocity
-ros2 topic echo /mavros/local_position/velocity_local
-
-# View IMU
-ros2 topic echo /mavros/imu/data
+ros2 run uav_ugv_simulation mission_planner
 ```
 
-## Flight Modes
+Send commands:
+```bash
+# Load demo mission
+ros2 topic pub /mission/command std_msgs/String "data: 'load'"
 
-| Mode | Description |
-|------|-------------|
-| STABILIZE | Manual control with attitude stabilization |
-| ALT_HOLD | Maintain altitude, manual position |
-| LOITER | Hold position and altitude |
-| GUIDED | Accept position commands |
-| AUTO | Follow pre-planned mission |
-| RTL | Return to launch point |
-| LAND | Controlled descent and landing |
+# Start mission
+ros2 topic pub /mission/command std_msgs/String "data: 'start'"
+
+# Pause/Resume
+ros2 topic pub /mission/command std_msgs/String "data: 'pause'"
+ros2 topic pub /mission/command std_msgs/String "data: 'resume'"
+
+# Abort
+ros2 topic pub /mission/command std_msgs/String "data: 'abort'"
+```
 
 ## Stopping the Simulation
 
-Press `Ctrl+C` in the terminal running the simulation.
-
-Or run:
 ```bash
+# Kill all processes
 bash scripts/kill_simulation.sh
+
+# Or press Ctrl+C in the terminal running the simulation
 ```
 
-## Camera Topics
+## Configuration Files
 
-The UAV has two cameras:
+| File | Description |
+|------|-------------|
+| `config/uav_params.yaml` | UAV navigation/vision parameters |
+| `config/ugv_params.yaml` | UGV motion parameters |
+| `config/mavros_params.yaml` | MAVROS connection settings |
+| `config/geofence_params.yaml` | Geofence boundaries |
+| `config/ardupilot_gps_denied.parm` | ArduPilot EKF configuration |
 
-```bash
-# Forward camera (visual odometry)
-ros2 topic echo /uav/camera/forward/image_raw
+## Next Steps
 
-# Downward camera (optical flow)
-ros2 topic echo /uav/camera/downward/image_raw
-```
-
-## GPS-Denied Navigation
-
-All position commands use local coordinates (meters from takeoff point):
-- X: North
-- Y: East  
-- Z: Up (or Down for NED frame)
-
-GPS is only used for geofencing boundaries, not for navigation.
+- [Architecture Overview](architecture.md)
+- [GPS-Denied Navigation](gps_denied_navigation.md)
+- [Troubleshooting](troubleshooting.md)
diff --git a/docs/wsl_setup_guide.md b/docs/wsl_setup_guide.md
index 8f0c05e..dc5c78c 100644
--- a/docs/wsl_setup_guide.md
+++ b/docs/wsl_setup_guide.md
@@ -1,129 +1,187 @@
 # WSL Setup Guide
 
-Setup guide for Windows Subsystem for Linux (WSL2).
+## Overview
 
-## Prerequisites
+This guide covers running the UAV-UGV simulation on Windows Subsystem for Linux (WSL2).
 
-- Windows 10 (version 21H2+) or Windows 11
+## Requirements
+
+- Windows 10 (21H2+) or Windows 11
 - WSL2 with Ubuntu 22.04
+- 16GB RAM minimum
+- Optional: NVIDIA GPU with WSL drivers
 
-### Install WSL2
+## WSL2 Installation
+
+### 1. Enable WSL2
 
 Open PowerShell as Administrator:
 
+```powershell
+wsl --install
+```
+
+### 2. Install Ubuntu
+
 ```powershell
 wsl --install -d Ubuntu-22.04
 ```
 
-Restart your computer, then open Ubuntu from the Start menu.
+### 3. Set WSL2 as Default
 
-## GUI Support
+```powershell
+wsl --set-default-version 2
+```
 
-### Windows 11 (WSLg)
+## Graphics Setup
 
-GUI works automatically. No additional setup needed.
+### Option A: WSLg (Windows 11 - Recommended)
 
-### Windows 10 (VcXsrv)
+WSLg is built into Windows 11 and works automatically.
 
-1. Download and install [VcXsrv](https://sourceforge.net/projects/vcxsrv/)
-2. Run XLaunch with these settings:
-   - Multiple windows
-   - Start no client
-   - Disable access control (checked)
-3. In WSL, set DISPLAY:
+Verify:
 ```bash
-export DISPLAY=$(cat /etc/resolv.conf | grep nameserver | awk '{print $2}'):0
+echo $DISPLAY
+# Should show something like :0
+```
+
+### Option B: VcXsrv (Windows 10)
+
+1. Download VcXsrv: https://sourceforge.net/projects/vcxsrv/
+2. Launch with "Disable access control" checked
+3. In WSL:
+
+```bash
+export DISPLAY=$(grep nameserver /etc/resolv.conf | awk '{print $2}'):0
+echo "export DISPLAY=$(grep nameserver /etc/resolv.conf | awk '{print $2}'):0" >> ~/.bashrc
+```
+
+## GPU Support
+
+### NVIDIA GPU (Optional)
+
+1. Install NVIDIA drivers for WSL: https://developer.nvidia.com/cuda/wsl
+2. Verify in WSL:
+
+```bash
+nvidia-smi
+```
+
+If working, you can use hardware acceleration.
+
+### Software Rendering (No GPU)
+
+Most reliable for WSL:
+
+```bash
+export LIBGL_ALWAYS_SOFTWARE=1
+export GALLIUM_DRIVER=llvmpipe
 ```
 
 ## Installation
 
+Same as native Ubuntu:
+
 ```bash
-git clone https://git.sirblob.co/SirBlob/simulation.git
-cd simulation
+cd ~/sim/uav_ugv_simulation
 bash setup.sh
 ```
 
-The setup script automatically:
-- Detects WSL environment
-- Installs GUI support packages
-- Creates WSL environment file
-- Configures DISPLAY variable
+## Running Simulation
 
-## Running the Simulation
+**Always use software rendering on WSL:**
 
 ```bash
-cd ~/simulation
 source activate_venv.sh
-bash scripts/run_simulation.sh
+bash scripts/run_autonomous.sh --software-render --mission hover
 ```
 
-If graphics are slow or Gazebo crashes:
-```bash
-bash scripts/run_simulation.sh --software-render
+## Performance Tips
+
+### 1. Allocate More Memory
+
+Create/edit `%USERPROFILE%\.wslconfig`:
+
+```ini
+[wsl2]
+memory=12GB
+processors=4
+swap=8GB
 ```
 
+Restart WSL:
+```powershell
+wsl --shutdown
+```
+
+### 2. Use Fast Storage
+
+Run simulation from Windows filesystem only if needed:
+```bash
+# Faster (Linux filesystem)
+cd ~/sim/uav_ugv_simulation
+
+# Slower (Windows filesystem)
+cd /mnt/c/Projects/simulation
+```
+
+### 3. Disable Unnecessary Visuals
+
+In software rendering mode, Gazebo is slower. Consider:
+- Running headless: `--headless` flag
+- Reducing physics rate
+- Simpler world files
+
 ## Troubleshooting
 
-### Black screen or no display
+### "cannot open display"
 
-Check DISPLAY variable:
 ```bash
-echo $DISPLAY
+# For WSLg
+export DISPLAY=:0
+
+# For VcXsrv
+export DISPLAY=$(grep nameserver /etc/resolv.conf | awk '{print $2}'):0
 ```
 
-For WSLg (Windows 11), should be `:0`
-For VcXsrv (Windows 10), should be `<IP>:0`
+### Graphics freeze/crash
 
-Test with:
-```bash
-xcalc
-```
-
-### Gazebo crashes immediately
-
-Use software rendering:
 ```bash
+# Always use software rendering
 export LIBGL_ALWAYS_SOFTWARE=1
-bash scripts/run_simulation.sh
+export GALLIUM_DRIVER=llvmpipe
+bash scripts/run_autonomous.sh --software-render
 ```
 
-### OpenGL errors
-
-```bash
-export MESA_GL_VERSION_OVERRIDE=3.3
-export MESA_GLSL_VERSION_OVERRIDE=330
-```
-
-### VcXsrv connection refused
-
-1. Check Windows Firewall allows VcXsrv
-2. Ensure XLaunch is running
-3. Disable access control in XLaunch settings
-
 ### Slow performance
 
-- Close unnecessary Windows applications
-- Allocate more RAM to WSL in `.wslconfig`:
-```ini
-[wsl2]
-memory=8GB
-processors=4
-```
+- Increase WSL memory (see Performance Tips)
+- Use software rendering
+- Close other applications
 
-- Use software rendering flag
-
-## Environment Variables
-
-The `activate_venv.sh` script sets these automatically:
+### Network issues
 
 ```bash
-export DISPLAY=:0  # or IP:0 for VcXsrv
-export LIBGL_ALWAYS_INDIRECT=0
-export MESA_GL_VERSION_OVERRIDE=3.3
+# If MAVLink connection fails
+# Check Windows firewall allows WSL traffic
 ```
 
-## Uninstall
+## Quick Start Commands
 
 ```bash
-bash scripts/uninstall.sh --all
+# 1. Open Ubuntu terminal
+
+# 2. Navigate to project
+cd ~/sim/uav_ugv_simulation
+
+# 3. Activate environment
+source activate_venv.sh
+
+# 4. Run with software rendering
+bash scripts/run_autonomous.sh --software-render --mission hover
 ```
+
+## Related
+
+- [Setup Guide](setup_guide.md)
+- [Troubleshooting](troubleshooting.md)
diff --git a/launch/full_simulation.launch.py b/launch/full_simulation.launch.py
index 2e785e9..7058e07 100644
--- a/launch/full_simulation.launch.py
+++ b/launch/full_simulation.launch.py
@@ -31,17 +31,21 @@ def generate_launch_description():
         'use_ground_truth', default_value='true', description='Use Gazebo ground truth'
     )
     
+    # Gazebo Harmonic (gz sim) instead of Gazebo Classic
     gazebo = ExecuteProcess(
-        cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
+        cmd=[
+            'gz', 'sim', '-v4', '-r',
+            os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
+        ],
         output='screen',
         additional_env={
-            'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'),
-            'GAZEBO_RESOURCE_PATH': f"{pkg_share}/worlds:" + os.path.expanduser('~/ardupilot_gazebo/worlds')
+            'GZ_SIM_RESOURCE_PATH': f"{pkg_share}/models:{os.path.expanduser('~/ardupilot_gazebo/models')}:{os.path.expanduser('~/ardupilot_gazebo/worlds')}",
+            'GZ_SIM_SYSTEM_PLUGIN_PATH': os.path.expanduser('~/ardupilot_gazebo/build')
         }
     )
     
     ardupilot_uav = TimerAction(
-        period=3.0,
+        period=5.0,  # Wait for Gazebo to initialize
         actions=[
             ExecuteProcess(
                 cmd=[
@@ -49,20 +53,20 @@ def generate_launch_description():
                     '-v', 'ArduCopter',
                     '-f', 'gazebo-iris',
                     '--model', 'JSON',
-                    '--map', '--console',
+                    '--no-mavproxy',  # Don't start MAVProxy, MAVROS will connect
                     '-I0',
-                    '--out', '127.0.0.1:14550',
-                    '--out', '127.0.0.1:14551',
-                    '--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
                 ],
-                cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
-                output='screen'
+                cwd=os.path.expanduser('~/ardupilot'),
+                output='screen',
+                additional_env={
+                    'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
+                }
             )
         ]
     )
     
     mavros_uav = TimerAction(
-        period=8.0,
+        period=15.0,  # Wait for ArduPilot SITL to start
         actions=[
             Node(
                 package='mavros',
@@ -73,7 +77,7 @@ def generate_launch_description():
                 parameters=[
                     os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
                     {
-                        'fcu_url': 'udp://:14550@127.0.0.1:14555',
+                        'fcu_url': 'tcp://127.0.0.1:5760',  # SITL TCP port
                         'gcs_url': '',
                         'target_system_id': 1,
                         'target_component_id': 1,
diff --git a/launch/uav_only.launch.py b/launch/uav_only.launch.py
index 2014b14..c4ca0d5 100644
--- a/launch/uav_only.launch.py
+++ b/launch/uav_only.launch.py
@@ -14,36 +14,42 @@ def generate_launch_description():
     
     world_arg = DeclareLaunchArgument(
         'world',
-        default_value=os.path.join(pkg_share, 'worlds', 'empty_custom.world'),
-        description='Path to world file'
+        default_value='iris_runway.sdf',
+        description='World file name'
     )
     
+    # Gazebo Harmonic
     gazebo = ExecuteProcess(
-        cmd=['gazebo', '--verbose', LaunchConfiguration('world')],
+        cmd=[
+            'gz', 'sim', '-v4', '-r',
+            os.path.expanduser('~/ardupilot_gazebo/worlds/iris_runway.sdf')
+        ],
         output='screen',
         additional_env={
-            'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'),
+            'GZ_SIM_RESOURCE_PATH': f"{pkg_share}/models:{os.path.expanduser('~/ardupilot_gazebo/models')}:{os.path.expanduser('~/ardupilot_gazebo/worlds')}",
+            'GZ_SIM_SYSTEM_PLUGIN_PATH': os.path.expanduser('~/ardupilot_gazebo/build')
         }
     )
     
     ardupilot = TimerAction(
-        period=3.0,
+        period=5.0,
         actions=[
             ExecuteProcess(
                 cmd=[
                     'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris',
-                    '--model', 'JSON', '--map', '--console', '-I0',
-                    '--out', '127.0.0.1:14550',
-                    '--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm')
+                    '--model', 'JSON', '--no-mavproxy', '-I0',
                 ],
-                cwd=os.path.expanduser('~/ardupilot/ArduCopter'),
-                output='screen'
+                cwd=os.path.expanduser('~/ardupilot'),
+                output='screen',
+                additional_env={
+                    'PATH': os.environ.get('PATH', '') + ':' + os.path.expanduser('~/ardupilot/Tools/autotest')
+                }
             )
         ]
     )
     
     mavros = TimerAction(
-        period=8.0,
+        period=15.0,
         actions=[
             Node(
                 package='mavros',
@@ -53,7 +59,7 @@ def generate_launch_description():
                 output='screen',
                 parameters=[
                     os.path.join(pkg_share, 'config', 'mavros_params.yaml'),
-                    {'fcu_url': 'udp://:14550@127.0.0.1:14555'}
+                    {'fcu_url': 'tcp://127.0.0.1:5760'}
                 ]
             )
         ]
diff --git a/scripts/kill_simulation.sh b/scripts/kill_simulation.sh
index f4f0d12..d8c5dba 100755
--- a/scripts/kill_simulation.sh
+++ b/scripts/kill_simulation.sh
@@ -2,6 +2,8 @@
 
 echo "Killing all simulation processes..."
 
+pkill -9 -f "gz sim" 2>/dev/null || true
+pkill -9 -f "ruby" 2>/dev/null || true  # gz sim uses ruby
 pkill -9 -f "gazebo" 2>/dev/null || true
 pkill -9 -f "gzserver" 2>/dev/null || true
 pkill -9 -f "gzclient" 2>/dev/null || true
@@ -10,6 +12,8 @@ pkill -9 -f "mavproxy" 2>/dev/null || true
 pkill -9 -f "mavros" 2>/dev/null || true
 pkill -9 -f "ArduCopter" 2>/dev/null || true
 pkill -9 -f "ArduRover" 2>/dev/null || true
+pkill -9 -f "arducopter" 2>/dev/null || true
+pkill -9 -f "autonomous_controller" 2>/dev/null || true
 pkill -9 -f "ros2" 2>/dev/null || true
 
 sleep 1
diff --git a/scripts/run_autonomous.sh b/scripts/run_autonomous.sh
new file mode 100755
index 0000000..a945ccf
--- /dev/null
+++ b/scripts/run_autonomous.sh
@@ -0,0 +1,122 @@
+#!/bin/bash
+# Autonomous UAV-UGV Simulation Runner
+# Launches Gazebo + ArduPilot SITL + Autonomous Controller
+
+set -e
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
+
+# Colors
+RED='\033[0;31m'
+GREEN='\033[0;32m'
+YELLOW='\033[1;33m'
+CYAN='\033[0;36m'
+NC='\033[0m'
+
+print_info() { echo -e "${CYAN}[INFO]${NC} $1"; }
+print_success() { echo -e "${GREEN}[SUCCESS]${NC} $1"; }
+print_error() { echo -e "${RED}[ERROR]${NC} $1"; }
+
+# Parse arguments
+SOFTWARE_RENDER=false
+WORLD="iris_runway.sdf"
+MISSION="hover"  # hover, square, circle
+
+while [[ $# -gt 0 ]]; do
+    case $1 in
+        --software-render) SOFTWARE_RENDER=true; shift ;;
+        --world) WORLD="$2"; shift 2 ;;
+        --mission) MISSION="$2"; shift 2 ;;
+        -h|--help)
+            echo "Usage: $0 [options]"
+            echo "  --software-render   Use software rendering (WSL/no GPU)"
+            echo "  --world <file>      World file to load (default: iris_runway.sdf)"
+            echo "  --mission <type>    Mission type: hover, square, circle (default: hover)"
+            exit 0
+            ;;
+        *) shift ;;
+    esac
+done
+
+# Cleanup function
+cleanup() {
+    print_info "Cleaning up..."
+    pkill -f "gz sim" 2>/dev/null || true
+    pkill -f "arducopter" 2>/dev/null || true
+    pkill -f "sim_vehicle.py" 2>/dev/null || true
+    pkill -f "autonomous_controller.py" 2>/dev/null || true
+}
+trap cleanup EXIT
+
+# Setup environment
+export PATH=$PATH:$HOME/ardupilot/Tools/autotest:$HOME/.local/bin
+export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds
+export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
+
+if [ "$SOFTWARE_RENDER" = true ]; then
+    print_info "Using software rendering (Mesa)"
+    export LIBGL_ALWAYS_SOFTWARE=1
+    export GALLIUM_DRIVER=llvmpipe
+fi
+
+# Kill any existing processes
+cleanup 2>/dev/null
+
+print_info "==================================="
+print_info "  Autonomous UAV-UGV Simulation"
+print_info "==================================="
+print_info "World: $WORLD"
+print_info "Mission: $MISSION"
+echo ""
+
+# Step 1: Start Gazebo
+print_info "Starting Gazebo Harmonic..."
+gz sim -v4 -r $HOME/ardupilot_gazebo/worlds/$WORLD &
+GZ_PID=$!
+sleep 5
+
+# Check if Gazebo started
+if ! kill -0 $GZ_PID 2>/dev/null; then
+    print_error "Gazebo failed to start"
+    exit 1
+fi
+print_success "Gazebo running (PID: $GZ_PID)"
+
+# Step 2: Start ArduPilot SITL
+print_info "Starting ArduPilot SITL..."
+cd ~/ardupilot
+sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 &
+SITL_PID=$!
+sleep 10
+
+# Check if SITL started
+if ! kill -0 $SITL_PID 2>/dev/null; then
+    print_error "ArduPilot SITL failed to start"
+    exit 1
+fi
+print_success "ArduPilot SITL running (PID: $SITL_PID)"
+
+# Step 3: Start the autonomous controller
+print_info "Starting Autonomous Controller..."
+print_info "Mission: $MISSION"
+
+python3 "$PROJECT_DIR/src/autonomous_controller.py" --mission "$MISSION" &
+CTRL_PID=$!
+
+print_success "Autonomous Controller started (PID: $CTRL_PID)"
+print_info ""
+print_info "==================================="
+print_info "  Simulation Running"
+print_info "==================================="
+print_info "The UAV will automatically:"
+print_info "  1. Wait for EKF initialization"
+print_info "  2. Arm and enter GUIDED mode"
+print_info "  3. Take off to 5 meters"
+print_info "  4. Execute mission: $MISSION"
+print_info ""
+print_info "Press Ctrl+C to stop"
+print_info "==================================="
+
+# Wait for controller to finish or user interrupt
+wait $CTRL_PID
diff --git a/scripts/setup_gazebo_nvidia.sh b/scripts/setup_gazebo_nvidia.sh
new file mode 100755
index 0000000..ef4bcd8
--- /dev/null
+++ b/scripts/setup_gazebo_nvidia.sh
@@ -0,0 +1,36 @@
+#!/bin/bash
+# Setup script for NVIDIA GPU with Gazebo
+
+set -e
+
+echo "Setting up NVIDIA GPU for Gazebo..."
+
+# Check if NVIDIA GPU is available
+if ! command -v nvidia-smi &> /dev/null; then
+    echo "NVIDIA driver not found. Please install NVIDIA drivers first."
+    exit 1
+fi
+
+nvidia-smi
+
+# Set environment variables for NVIDIA
+export __NV_PRIME_RENDER_OFFLOAD=1
+export __GLX_VENDOR_LIBRARY_NAME=nvidia
+
+# For Gazebo Harmonic
+export LIBGL_ALWAYS_SOFTWARE=0
+
+# Create persistent config
+cat >> ~/.bashrc << 'EOF'
+
+# NVIDIA GPU for Gazebo
+export __NV_PRIME_RENDER_OFFLOAD=1
+export __GLX_VENDOR_LIBRARY_NAME=nvidia
+export LIBGL_ALWAYS_SOFTWARE=0
+EOF
+
+echo ""
+echo "NVIDIA setup complete!"
+echo "Please restart your terminal or run: source ~/.bashrc"
+echo ""
+echo "Test with: glxinfo | grep 'OpenGL renderer'"
diff --git a/src/autonomous_controller.py b/src/autonomous_controller.py
new file mode 100755
index 0000000..00d35fb
--- /dev/null
+++ b/src/autonomous_controller.py
@@ -0,0 +1,368 @@
+#!/usr/bin/env python3
+"""
+Autonomous UAV Controller using pymavlink.
+Connects directly to ArduPilot SITL and controls the drone autonomously.
+No ROS/MAVROS required.
+"""
+
+import time
+import math
+import argparse
+from pymavlink import mavutil
+
+
+class AutonomousController:
+    """Autonomous UAV controller using pymavlink."""
+    
+    def __init__(self, connection_string='tcp:127.0.0.1:5760'):
+        """Initialize connection to ArduPilot."""
+        print(f"[CTRL] Connecting to {connection_string}...")
+        self.mav = mavutil.mavlink_connection(connection_string)
+        
+        # Wait for heartbeat
+        print("[CTRL] Waiting for heartbeat...")
+        self.mav.wait_heartbeat()
+        print(f"[CTRL] Connected! System {self.mav.target_system} Component {self.mav.target_component}")
+        
+        self.home_position = None
+        
+    def wait_for_ekf(self, timeout=60):
+        """Wait for EKF to initialize."""
+        print("[CTRL] Waiting for EKF initialization...")
+        start_time = time.time()
+        
+        while time.time() - start_time < timeout:
+            # Request EKF status
+            msg = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=1)
+            if msg:
+                # Check if EKF is healthy (flags indicate position and velocity estimates are good)
+                if msg.flags & 0x01:  # EKF_ATTITUDE
+                    print(f"[CTRL] EKF Status: flags={msg.flags:#x}")
+                    if msg.flags >= 0x1FF:  # All basic flags set
+                        print("[CTRL] EKF initialized successfully!")
+                        return True
+            
+            # Also check for GPS status messages to see progress
+            msg = self.mav.recv_match(type='STATUSTEXT', blocking=False)
+            if msg:
+                text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
+                print(f"[SITL] {text}")
+                if 'EKF3 IMU' in text and 'initialised' in text:
+                    time.sleep(2)  # Give it a moment after EKF init
+                    return True
+                    
+        print("[CTRL] EKF initialization timeout!")
+        return False
+    
+    def set_mode(self, mode):
+        """Set flight mode."""
+        mode_mapping = self.mav.mode_mapping()
+        if mode not in mode_mapping:
+            print(f"[CTRL] Unknown mode: {mode}")
+            return False
+            
+        mode_id = mode_mapping[mode]
+        self.mav.mav.set_mode_send(
+            self.mav.target_system,
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            mode_id
+        )
+        
+        # Wait for ACK
+        for _ in range(10):
+            msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg and msg.custom_mode == mode_id:
+                print(f"[CTRL] Mode set to {mode}")
+                return True
+        
+        print(f"[CTRL] Failed to set mode {mode}")
+        return False
+    
+    def arm(self, force=True):
+        """Arm the vehicle."""
+        print("[CTRL] Arming...")
+        
+        # Disable arming checks if force
+        if force:
+            self.mav.mav.param_set_send(
+                self.mav.target_system,
+                self.mav.target_component,
+                b'ARMING_CHECK',
+                0,
+                mavutil.mavlink.MAV_PARAM_TYPE_INT32
+            )
+            time.sleep(0.5)
+        
+        self.mav.mav.command_long_send(
+            self.mav.target_system,
+            self.mav.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,  # confirmation
+            1,  # arm
+            21196 if force else 0,  # force arm magic number
+            0, 0, 0, 0, 0
+        )
+        
+        # Wait for arm
+        for _ in range(30):
+            msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg and msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
+                print("[CTRL] Armed successfully!")
+                return True
+                
+            # Check for failure messages
+            ack = self.mav.recv_match(type='COMMAND_ACK', blocking=False)
+            if ack and ack.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
+                if ack.result != mavutil.mavlink.MAV_RESULT_ACCEPTED:
+                    print(f"[CTRL] Arm rejected: {ack.result}")
+                    
+        print("[CTRL] Arm timeout")
+        return False
+    
+    def takeoff(self, altitude=5.0):
+        """Take off to specified altitude."""
+        print(f"[CTRL] Taking off to {altitude}m...")
+        
+        self.mav.mav.command_long_send(
+            self.mav.target_system,
+            self.mav.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0,
+            0, 0, 0, 0, 0, 0,
+            altitude
+        )
+        
+        # Wait for takeoff
+        start_time = time.time()
+        while time.time() - start_time < 30:
+            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            if msg:
+                current_alt = msg.relative_alt / 1000.0  # mm to m
+                print(f"[CTRL] Altitude: {current_alt:.1f}m / {altitude}m")
+                if current_alt >= altitude * 0.9:
+                    print("[CTRL] Takeoff complete!")
+                    return True
+                    
+        print("[CTRL] Takeoff timeout")
+        return False
+    
+    def goto_local(self, north, east, down, yaw=0):
+        """Go to local position (NED frame)."""
+        print(f"[CTRL] Going to N:{north:.1f} E:{east:.1f} D:{down:.1f}")
+        
+        # Use SET_POSITION_TARGET_LOCAL_NED
+        self.mav.mav.set_position_target_local_ned_send(
+            0,  # timestamp
+            self.mav.target_system,
+            self.mav.target_component,
+            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+            0b0000111111111000,  # position only mask
+            north, east, down,  # position
+            0, 0, 0,  # velocity
+            0, 0, 0,  # acceleration
+            yaw, 0   # yaw, yaw_rate
+        )
+        
+    def wait_for_position(self, north, east, down, tolerance=1.0, timeout=30):
+        """Wait until vehicle reaches position."""
+        start_time = time.time()
+        
+        while time.time() - start_time < timeout:
+            msg = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+            if msg:
+                dist = math.sqrt(
+                    (msg.x - north)**2 + 
+                    (msg.y - east)**2 + 
+                    (msg.z - down)**2
+                )
+                print(f"[CTRL] Position: N:{msg.x:.1f} E:{msg.y:.1f} D:{msg.z:.1f} (dist:{dist:.1f})")
+                if dist < tolerance:
+                    return True
+                    
+        return False
+    
+    def land(self):
+        """Land the vehicle."""
+        print("[CTRL] Landing...")
+        
+        self.mav.mav.command_long_send(
+            self.mav.target_system,
+            self.mav.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_LAND,
+            0,
+            0, 0, 0, 0, 0, 0, 0
+        )
+        
+        # Wait for land
+        start_time = time.time()
+        while time.time() - start_time < 60:
+            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            if msg:
+                current_alt = msg.relative_alt / 1000.0
+                print(f"[CTRL] Landing... Alt: {current_alt:.1f}m")
+                if current_alt < 0.3:
+                    print("[CTRL] Landed!")
+                    return True
+                    
+        return False
+    
+    def disarm(self):
+        """Disarm the vehicle."""
+        print("[CTRL] Disarming...")
+        
+        self.mav.mav.command_long_send(
+            self.mav.target_system,
+            self.mav.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,
+            0,  # disarm
+            21196,  # force
+            0, 0, 0, 0, 0
+        )
+        time.sleep(1)
+        print("[CTRL] Disarmed")
+        
+    def run_hover_mission(self, altitude=5.0, duration=30):
+        """Simple hover mission."""
+        print(f"\n[MISSION] Hover at {altitude}m for {duration}s")
+        
+        if not self.wait_for_ekf():
+            return False
+            
+        if not self.set_mode('GUIDED'):
+            return False
+            
+        if not self.arm(force=True):
+            return False
+            
+        if not self.takeoff(altitude):
+            return False
+            
+        print(f"[MISSION] Hovering for {duration} seconds...")
+        time.sleep(duration)
+        
+        self.land()
+        self.disarm()
+        
+        print("[MISSION] Hover mission complete!")
+        return True
+    
+    def run_square_mission(self, altitude=5.0, side=5.0):
+        """Fly a square pattern."""
+        print(f"\n[MISSION] Square pattern: {side}m sides at {altitude}m")
+        
+        if not self.wait_for_ekf():
+            return False
+            
+        if not self.set_mode('GUIDED'):
+            return False
+            
+        if not self.arm(force=True):
+            return False
+            
+        if not self.takeoff(altitude):
+            return False
+        
+        # Square waypoints (NED: North, East, Down)
+        waypoints = [
+            (side, 0, -altitude),     # North
+            (side, side, -altitude),  # North-East
+            (0, side, -altitude),     # East
+            (0, 0, -altitude),        # Back to start
+        ]
+        
+        for i, (n, e, d) in enumerate(waypoints):
+            print(f"\n[MISSION] Waypoint {i+1}/4: N:{n} E:{e}")
+            self.goto_local(n, e, d)
+            time.sleep(0.5)  # Let command process
+            self.wait_for_position(n, e, d, tolerance=1.5, timeout=20)
+            time.sleep(2)  # Hover at waypoint
+        
+        self.land()
+        self.disarm()
+        
+        print("[MISSION] Square mission complete!")
+        return True
+    
+    def run_circle_mission(self, altitude=5.0, radius=5.0, points=8):
+        """Fly a circular pattern."""
+        print(f"\n[MISSION] Circle pattern: {radius}m radius at {altitude}m")
+        
+        if not self.wait_for_ekf():
+            return False
+            
+        if not self.set_mode('GUIDED'):
+            return False
+            
+        if not self.arm(force=True):
+            return False
+            
+        if not self.takeoff(altitude):
+            return False
+        
+        # Generate circle waypoints
+        for i in range(points + 1):  # +1 to complete the circle
+            angle = 2 * math.pi * i / points
+            north = radius * math.cos(angle)
+            east = radius * math.sin(angle)
+            
+            print(f"\n[MISSION] Circle point {i+1}/{points+1}")
+            self.goto_local(north, east, -altitude)
+            time.sleep(0.5)
+            self.wait_for_position(north, east, -altitude, tolerance=1.5, timeout=15)
+            time.sleep(1)
+        
+        # Return to center
+        self.goto_local(0, 0, -altitude)
+        self.wait_for_position(0, 0, -altitude, tolerance=1.5, timeout=15)
+        
+        self.land()
+        self.disarm()
+        
+        print("[MISSION] Circle mission complete!")
+        return True
+
+
+def main():
+    parser = argparse.ArgumentParser(description='Autonomous UAV Controller')
+    parser.add_argument('--connection', default='tcp:127.0.0.1:5760',
+                        help='MAVLink connection string')
+    parser.add_argument('--mission', choices=['hover', 'square', 'circle'],
+                        default='hover', help='Mission type')
+    parser.add_argument('--altitude', type=float, default=5.0,
+                        help='Flight altitude in meters')
+    parser.add_argument('--size', type=float, default=5.0,
+                        help='Mission size (side length or radius)')
+    parser.add_argument('--duration', type=float, default=30.0,
+                        help='Hover duration in seconds')
+    args = parser.parse_args()
+    
+    print("=" * 50)
+    print("  Autonomous UAV Controller")
+    print("=" * 50)
+    print(f"  Connection: {args.connection}")
+    print(f"  Mission: {args.mission}")
+    print(f"  Altitude: {args.altitude}m")
+    print("=" * 50)
+    print()
+    
+    try:
+        controller = AutonomousController(args.connection)
+        
+        if args.mission == 'hover':
+            controller.run_hover_mission(args.altitude, args.duration)
+        elif args.mission == 'square':
+            controller.run_square_mission(args.altitude, args.size)
+        elif args.mission == 'circle':
+            controller.run_circle_mission(args.altitude, args.size)
+            
+    except KeyboardInterrupt:
+        print("\n[CTRL] Interrupted by user")
+    except Exception as e:
+        print(f"[ERROR] {e}")
+        import traceback
+        traceback.print_exc()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/control/uav_controller.py b/src/control/uav_controller.py
index 69e85e0..f3df3b7 100644
--- a/src/control/uav_controller.py
+++ b/src/control/uav_controller.py
@@ -29,16 +29,24 @@ class UAVController(Node):
         self.declare_parameter('takeoff_altitude', 5.0)
         self.declare_parameter('position_tolerance', 0.3)
         self.declare_parameter('namespace', '/uav')
+        self.declare_parameter('auto_arm', True)
+        self.declare_parameter('auto_takeoff', True)
+        self.declare_parameter('startup_delay', 15.0)  # Wait for EKF
         
         self.takeoff_altitude = self.get_parameter('takeoff_altitude').value
         self.position_tolerance = self.get_parameter('position_tolerance').value
         ns = self.get_parameter('namespace').value
+        self.auto_arm = self.get_parameter('auto_arm').value
+        self.auto_takeoff = self.get_parameter('auto_takeoff').value
+        self.startup_delay = self.get_parameter('startup_delay').value
         
         self.state = FlightState.DISARMED
         self.mavros_state = None
         self.current_pose = None
         self.target_pose = None
         self.home_position = None
+        self.startup_complete = False
+        self.ekf_ready = False
         
         self.state_sub = self.create_subscription(
             State, f'{ns}/mavros/state', self.state_callback, 10)
@@ -72,8 +80,55 @@ class UAVController(Node):
         self.setpoint_timer = self.create_timer(0.05, self.publish_setpoint)
         self.status_timer = self.create_timer(0.5, self.publish_status)
         
+        # Delayed auto-start (wait for EKF initialization)
+        if self.auto_arm or self.auto_takeoff:
+            self.get_logger().info(f'Auto-mission enabled. Starting in {self.startup_delay}s...')
+            self.startup_timer = self.create_timer(self.startup_delay, self.auto_startup)
+        
         self.get_logger().info('UAV Controller Started - GPS-denied mode')
     
+    def auto_startup(self):
+        """Auto arm and takeoff after startup delay."""
+        # Only run once
+        if self.startup_complete:
+            return
+        self.startup_complete = True
+        self.startup_timer.cancel()
+        
+        self.get_logger().info('Auto-startup: Beginning autonomous sequence...')
+        
+        # Check if MAVROS is connected
+        if self.mavros_state is None:
+            self.get_logger().warn('MAVROS not connected yet, retrying in 5s...')
+            self.startup_timer = self.create_timer(5.0, self.auto_startup)
+            self.startup_complete = False
+            return
+        
+        # Set GUIDED mode first
+        self.get_logger().info('Auto-startup: Setting GUIDED mode...')
+        self.set_mode('GUIDED')
+        
+        # Wait a moment then arm
+        self.create_timer(2.0, self._auto_arm_callback, callback_group=None)
+    
+    def _auto_arm_callback(self):
+        """Callback to arm after mode is set."""
+        if self.auto_arm:
+            self.get_logger().info('Auto-startup: Arming...')
+            self.arm()
+        
+        # Wait then takeoff
+        if self.auto_takeoff:
+            self.create_timer(3.0, self._auto_takeoff_callback, callback_group=None)
+    
+    def _auto_takeoff_callback(self):
+        """Callback to takeoff after arming."""
+        if self.mavros_state and self.mavros_state.armed:
+            self.get_logger().info('Auto-startup: Taking off...')
+            self.takeoff()
+        else:
+            self.get_logger().warn('Auto-startup: Not armed, cannot takeoff')
+    
     def state_callback(self, msg):
         self.mavros_state = msg
         if msg.armed and self.state == FlightState.DISARMED: