simulation/docs/architecture.md

System Architecture

Overview

┌─────────────────────────────────────────────────────────────────┐
│                        Simulation                                │
├───────────────────┬─────────────────────┬───────────────────────┤
│   Gazebo Harmonic │   ArduPilot SITL    │    ROS 2 Nodes        │
│   (Physics/3D)    │   (Flight Control)  │    (Perception/Nav)   │
└───────────────────┴─────────────────────┴───────────────────────┘
         │                    │                      │
         └────────────────────┼──────────────────────┘
                              │
                    ┌─────────┴─────────┐
                    │   MAVLink/MAVROS  │
                    └───────────────────┘

Components

1. Gazebo Harmonic

Role: 3D simulation, physics, sensors

• World simulation: Ground plane, lighting, physics
• UAV model: Iris quadcopter with cameras
• UGV model: Differential drive rover
• Sensors: Cameras, IMU, rangefinder

2. ArduPilot SITL

Role: Flight controller simulation

• EKF3: State estimation using external vision
• Flight modes: GUIDED, LOITER, RTL, LAND
• Motor mixing: Quadcopter dynamics
• Failsafe: Battery, geofence, communication

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

3. ROS 2 Nodes

Vision Pipeline

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
odom	Local origin (takeoff point)
base_link	Vehicle body frame
map	World frame (aligned with odom)

NED Convention:

• X = North (forward)
• Y = East (right)
• Z = Down (negative altitude)

File Structure

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

Configuration

ArduPilot EKF Sources

EK3_SRC1_POSXY: 6  # External Nav
EK3_SRC1_POSZ: 1   # Barometer
EK3_SRC1_VELXY: 6  # External Nav
EK3_SRC1_YAW: 6    # External Nav

Sensor Weights

vo_weight: 0.6      # Visual odometry
optical_flow: 0.3   # Optical flow
imu: 0.1            # IMU integration