Theory & Methodology

A self-contained treatment of the control systems theory behind the UAV swarm simulation: state-space dynamics, LQR optimal control, artificial potential fields, graph Laplacian consensus, and stochastic wind rejection. Every gain and tuning parameter is explained in context.

1. Control Architecture Overview

The swarm uses a cascaded guidance-control architecture. An outer APF guidance layer generates a desired velocity for each agent. A middle consensus layer adjusts that velocity to preserve formation geometry. An inner LQR state-feedback controller computes thrust commands to track the desired velocity. Wind gusts enter at the plant level as force disturbances.

  Waypoints → [APF Guidance] → v_des → +[Consensus] → v_cmd → [LQR] → u → [Quadcopter] → x
                                       ↑                              ↑
                                    Neighbors                         Wind Gust
  

2. Quadcopter State-Space Dynamics

Each agent is modeled as a double-integrator with linear aerodynamic drag, linearized around a steady hover trim condition. The state vector tracks 3D position and velocity:

The continuous-time state-space dynamics:

Where \(m = 1.2\) kg is the vehicle mass, \(b = 0.15\) Ns/m is the aerodynamic drag coefficient, \(\mathbf{u}_i \in \mathbb{R}^3\) is the commanded thrust vector, and \(\mathbf{d}_i(t)\) represents wind gust perturbations. The damping ratio \(\zeta = b/(2m) \approx 0.0625\) gives a lightly damped plant, motivating the need for active state feedback.

# Fourth-order Runge-Kutta integration at 50 Hz
def step_rk4(self, dt, u_cmd):
    def f(s):
        old = self.state.copy()
        self.state = s
        d = self.derivatives(u_cmd)
        self.state = old
        return d
    s0 = self.state.copy()
    k1 = f(s0)
    k2 = f(s0 + 0.5 * dt * k1)
    k3 = f(s0 + 0.5 * dt * k2)
    k4 = f(s0 + dt * k3)
    self.state = s0 + (dt / 6.0) * (k1 + 2.0*k2 + 2.0*k3 + k4)

Python implementation of the RK4 integrator. The nested derivative wrapper evaluates \(\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}\) at each stage without mutating the agent state.

3. Guidance Layer: Artificial Potential Fields

The outer loop computes a desired velocity using the negative gradient of a total potential energy function. Three potentials are superposed: target attraction, obstacle repulsion, and wall confinement.

3.1 Target Attraction

The attractive potential is a quadratic well centered at the current waypoint. In the 3D simulation, the leader follows a sequence of 5 waypoints; followers track a virtual target offset from the leader via the formation geometry. In the 2D corridor simulation, attraction is proportional to distance via gain \(K_{\text{ATT}}\):

The gain \(K_{\text{ATT}} = 3.0 / 8.0 = 0.375\) s\(^{-1}\) ensures the drone cruises at \(\approx 3\) m/s when far from the target and slows down smoothly as it approaches. This avoids the overshoot-and-oscillate behavior of a constant-speed approach.

In the 3D simulation, the leader uses a unit-vector velocity command with cruise speed \(v_{\text{cruise}} = 3.0\) m/s and a follower velocity-matching term with gain \(k_{\text{follow}} = 1.5\) s\(^{-1}\):

The gain \(k_{\text{follow}}\) acts as a proportional position corrector: at a 1 m position error, it adds 1.5 m/s to the desired velocity. Higher values produce tighter formation tracking but amplify noise from the leader's velocity signal.

3.2 Obstacle Repulsion

Each spherical obstacle generates a repulsive potential that activates when the drone's signed distance \(\rho_k\) to the sphere surface falls below the threshold \(\rho_0\):

Gain tuning. The repulsion gain \(k_{\text{avoid}}\) controls how sharply the drone turns away from obstacles. In the 3D simulation, \(k_{\text{avoid}} = 30.0\) (eta = 30) with \(\rho_0 = 3.5\) m produces firm avoidance at 1-2 m standoff. In the 2D corridor, \(k_{\text{avoid}} = 4.0\) with \(\rho_0 = 3.5\) m is used because the confined space (Y ±5 m) cannot accommodate aggressive lateral dodges. The gradient magnitude grows as \(1/\rho_k^3\) near the surface, so a hard clip at 8 m/s prevents numerical blowup.

# APF repulsion: force vector from each activated obstacle
def compute_apf_repulsion(pos, obstacles, rho0, ka):
    f_total = np.zeros(2)  # 2D corridor version
    for obs in obstacles:
        delta = pos - obs.center
        rho = np.linalg.norm(delta) - obs.radius
        if 0 < rho < rho0:
            n_hat = delta / np.linalg.norm(delta)
            scale = ka * (1.0/rho - 1.0/rho0) / (rho * rho)
            f_total += np.clip(scale * n_hat, -8.0, 8.0)
    return f_total

The repulsive force grows as \(1/\rho^3\) near an obstacle surface. The 8 m/s clip prevents numerical blowup at very close range.

3.3 Wall Confinement (2D Corridor Only)

The 2D corridor has hard boundaries at X = ±15 m and Y = ±5 m. A repulsive wall potential with the same functional form as Eq. 6 pushes the drone back toward the corridor center when within 2 m of a wall:

With \(k_{\text{wall}} = 3.0\) and \(d_{\text{margin}} = 2.0\) m, the wall force remains a gentle perturbation (comparable to \(v_{\text{cruise}}\)) during normal operation but rises sharply if the drone approaches within 0.5 m of a boundary. A hard position clamp at ±0.1 m from the wall acts as a safety net.

4. Formation Control via Graph Laplacian Consensus

The formation layer is a decentralized consensus protocol operating on the graph Laplacian of the communication topology. Every agent shares its position with neighbors within \(R_{\text{comm}} = 8\) m. The consensus force drives the relative offsets between agents toward the desired formation geometry:

The gain \(k_c = 2.0\) controls the stiffness of the virtual spring connecting each agent pair. A higher \(k_c\) produces faster convergence to the formation but amplifies relative position noise and can cause oscillatory transients during obstacle avoidance.

4.1 3D DCM Formation Rotation

As the leader turns, climbs, and dives through the debris field, the wedge offsets are continuously rotated in 3D so the formation stays aligned with the direction of travel. A Direction Cosine Matrix is constructed from the leader's velocity vector:

The leader's velocity is low-pass filtered with \(\alpha = 0.05\) to prevent wind gusts or avoidance jerks from causing formation snapping. The reference up-vector \(\hat{\mathbf{k}}_{\text{ref}}\) switches from +Y to +X during near-vertical climbs to maintain a well-conditioned rotation matrix.

# 3D DCM formation rotation aligned with leader velocity
def dcm_from_velocity(v, k_ref=np.array([0,1,0])):
    ux = v / np.linalg.norm(v)
    uz = np.cross(ux, k_ref)
    uz /= np.linalg.norm(uz)
    uy = np.cross(uz, ux)
    return np.column_stack([ux, uy, uz])  # rotation matrix R

offsets_rotated = R @ offsets_nominal.T  # apply to wedge geometry

The DCM transforms nominal formation offsets into body-frame coordinates aligned with the leader's velocity. The wedge stays oriented with the direction of travel regardless of altitude changes.

Nominal Wedge Offsets

5. Velocity Tracking via LQR Optimal Control

The inner loop is a full-state LQR regulator that tracks the velocity command from the guidance and consensus layers. Rather than tuning separate PID loops for each axis, the LQR framework produces optimal gains by solving the Continuous-Time Algebraic Riccati Equation (CARE):

The state error \(\mathbf{x}_e = \mathbf{x}_i - \mathbf{x}_{\text{cmd}}\) uses the current measured position and the commanded velocity (position error is zero; only velocity tracking matters for the guidance layer). The weighting matrices:

The diagonal entries of \(\mathbf{Q}\) prioritize position error (weight 10) ten times higher than velocity error (weight 1). This aggressive position weighting forces tight formation keeping. The control penalty \(\mathbf{R} = 0.1\) allows up to \(\approx 15\) N of thrust before the cost penalizes further effort — matching the physical \(T_{\text{max}}\) limit.

The optimal gain \(\mathbf{K}\) is obtained from the CARE solution:

The resulting gain matrix has the structure (shown for one axis):

With \(K_p \approx 10.0\) N/m and \(K_v \approx 5.68\) Ns/m. These can be interpreted as a proportional-derivative controller: a 1 m position error produces 10 N of corrective thrust; a 1 m/s velocity error produces 5.68 N. The natural frequency \(\omega_n = \sqrt{K_p/m} \approx 2.89\) rad/s gives a closed-loop bandwidth of about 0.46 Hz — fast enough for formation tracking but slow enough to avoid exciting structural modes.

The combined control law feeds the LQR output through a saturation block:

The 2D corridor simulation skips the LQR layer entirely and uses a pure velocity-command approach (Sections 3.1-3.3 directly produce the velocity). This is standard practice in path planning demonstrations where the focus is on the guidance law, not the inner-loop tracking dynamics.

def compute_lqr_gain(A, B, Q, R):
    """Solve CARE, return optimal gain matrix K (3×6)."""
    P = solve_continuous_are(A, B, Q, R)
    K = np.linalg.inv(R) @ B.T @ P
    return K

# Physical plant
A = np.block([[np.zeros((3,3)), np.eye(3)],
              [np.zeros((3,3)), -(b/m)*np.eye(3)]])
B = np.block([[np.zeros((3,3))],
              [(1.0/m)*np.eye(3)]])
K_opt = compute_lqr_gain(A, B, Q, R)  # K_p ≈ 10.0, K_v ≈ 5.68

The CARE solver from SciPy computes the optimal gain in a single call. The resulting \(K_p = 10.0\) N/m and \(K_v = 5.68\) Ns/m define a closed-loop bandwidth of 0.46 Hz.

6. Disturbance Rejection: Dryden Wind Turbulence

Atmospheric turbulence is modeled as a first-order Gauss-Markov process that matches the Dryden power spectral density, a standard specification for aerospace gust modeling (MIL-STD-1797A):

Turbulence intensity \(\sigma = 2.0\) m/s (light-to-moderate gusts, typical of fair-weather low-altitude flight) and correlation time \(\tau = 5.0\) s (gust length scale \(\approx 250\) m at typical UAV speeds). The discrete-time update is exact:

Wind enters the plant dynamics as a force disturbance \(\mathbf{d}_i(t) = \mathbf{w}(t)/m\) added to the acceleration. The consensus layer provides natural disturbance rejection: when a wind gust displaces a drone, the virtual springs connecting it to its neighbors generate restorative forces. This decentralized mechanism requires no explicit gust detection or mode switching — it emerges from the consensus feedback.

7. Lyapunov Stability Analysis

The closed-loop stability of the combined guidance-control system is verified using a Lyapunov candidate equal to the total potential energy plus kinetic energy:

Taking the time derivative along system trajectories:

The derivative is negative semidefinite because the cross terms from the APF gradient and the LQR position feedback cancel exactly — the guidance force applied to the plant is subtracted by the same force entering the LQR reference, a consequence of the cascaded architecture. Damping \(b\mathbf{I}\) and the LQR velocity gain \(\mathbf{K}_v\) ensure \(\dot{\mathcal{V}} = 0\) only when \(\dot{\mathbf{r}}_i = 0\) for all \(i\) (invoking LaSalle's invariance principle). Wind gusts appear as bounded disturbances \(\mathbf{d}_i(t)\) and produce bounded tracking errors that decay exponentially once the gust subsides.

8. Parameter Summary: Gains and K Values

The table below lists every tunable gain in the simulation, its numerical value, physical interpretation, and the rationale for its selection.

9. Flight Software Architecture

The Python simulation demonstrates guidance and control theory in a research-oriented setting. The C++ flight software stack translates these concepts into a production-grade architecture suitable for real embedded systems: a hand-rolled finite state machine for flight phase management, a multi-detector FDIR system for fault tolerance, a from-scratch 6-DOF dynamics kernel with quaternion kinematics, and a MAVLink-compatible telemetry protocol for ground station communication.

9.1 Finite State Machines for Flight Phases

Flight software requires deterministic phase management. The state machine uses 10 discrete states with guarded transitions, implemented as an explicit transition table rather than a library (e.g., Boost.SML). This hand-rolled approach provides full control over transition logic, avoids template metaprogramming complexity, and makes every state change auditable.

  IDLE → PRE_ARM → ARMED → TAKEOFF → CRUISE → WAYPOINT_TRACK
                                        → RETURN_TO_HOME → LAND → IDLE
  Any state → EMERGENCY → SAFE_MODE (on fault)
  

Guard conditions gate critical transitions. The IDLE → PRE_ARM transition requires four simultaneous preconditions: GPS lock, EKF health, battery voltage above threshold, and pre-arm checks passing. This prevents arming in a degraded state. The EMERGENCY → SAFE_MODE transition is unconditional—any event in EMERGENCY immediately enters SAFE_MODE, ensuring the system never latches in a hazardous state.

// Transition table: from_state + event + guards → to_state
void StateMachine::process_event(Event event) {
  switch (state_) {
    case State::IDLE:
      if (event == Event::ARM_CMD && guard_pre_arm()
          && guard_gps_lock() && guard_ekf_health()
          && guard_battery_ok())
        transition_to(State::PRE_ARM);
      break;
    case State::EMERGENCY:
      transition_to(State::SAFE_MODE);  // unconditional
      break;
    // ... 25+ transition rules
  }
}

The process_event method encodes the full transition table. Guards are only checked on the most critical transitions (IDLE → PRE_ARM); all other transitions are unconditional or single-guard.

9.2 FDIR: Fault Detection, Isolation & Recovery

Fault Detection, Isolation, and Recovery (FDIR) is the safety backbone of any flight software system. The architecture uses a facade pattern: FdirManager aggregates four independent detectors behind a single interface, each using a different detection mechanism to cover different failure modes.

The recovery hierarchy follows aerospace convention: sensor failover → subsystem isolation → EMERGENCY → SAFE_MODE. FdirManager uses latching semantics—the first fault detected becomes the primary fault and is never overwritten, even if subsequent faults occur. This preserves the root cause for post-flight analysis.

9.3 6-DOF Rigid Body Dynamics

The 6-DOF dynamics kernel models the full rigid body motion of a quadcopter using quaternion orientation representation (avoiding gimbal lock). The 13-element state vector:

Quaternion kinematics govern orientation evolution:

Translation includes gravity, aerodynamic drag, and thrust. The rotational equation captures gyroscopic coupling:

The inertia tensor for a quadcopter is diagonal (principal axes aligned with body frame):

Integration uses 4th-order Runge-Kutta at 100 Hz. The quaternion is integrated by averaging the derivatives from all four stages and renormalizing to unit length, preventing numerical drift:

// Quaternion derivative: dq/dt = 0.5 * q * [0, omega]
Quaternion quaternion_derivative(const Quaternion& q,
                                const std::array<float,3>& omega) {
  Quaternion q_omega(0, omega[0], omega[1], omega[2]);
  return q * q_omega * 0.5f;
}

// Gyroscopic term: omega x (I * omega)
auto Iw = inertia_matrix * omega;
auto gyroscopic = cross(omega, Iw);
auto domega = inverse(I) * (torque - gyroscopic);

The quaternion derivative follows the kinematic equation. The gyroscopic term omega x (I*omega) couples rotational axes and is critical for accurate attitude simulation during aggressive maneuvers.

9.4 MAVLink Binary Protocol

MAVLink v2 is the de facto standard for UAV communication. The implementation uses a minimal, self-contained encoder (no pymavlink dependency in C++) with packed structs for zero-copy serialization:

  [STX=0xFD] [LEN] [INC_FLAGS] [CMP_FLAGS] [SEQ] [SYS_ID] [CMP_ID] [MSG_ID:3] [PAYLOAD:N] [CRC8]
  ——————————————————————————————————————————————————
              10-byte header                              N bytes        1 byte
  

Four message types implement the telemetry stream:

CRC-8 with polynomial 0x07 provides per-packet integrity checking. The encoder auto-increments a sequence counter for packet ordering verification on the receiver side.