How does a 15-tonne surveillance drone stay in the air for 32 hours without falling out of the sky? Why does a quadrotor flip over when one motor fails? This module builds your physical intuition for how aircraft move — from the basic forces keeping them airborne, to the full six-degree-of-freedom equations that govern every twist and acceleration.
📚 REFERENCES: Marqués & Da Ronch §3–6 · Sadraey §4 · Bestaoui Sebbane §2
Four forces act on any flying vehicle at all times: Lift (pushes it up), Weight (pulls it down), Thrust (pushes it forward), and Drag (resists forward motion). Steady, level flight is simply the condition where these four forces are perfectly balanced — lift equals weight, thrust equals drag. The moment any of them change (engine fails, wind gust hits, pilot pulls the stick), the aircraft accelerates in some direction. Flight dynamics is the science of predicting exactly how and how fast.
For a UAV, these four forces are not independent — they all link back to two things: the aircraft's speed through the air, and its angle relative to that airflow. The dynamic pressure $\bar{q} = \frac{1}{2}\rho V^2$ is the fundamental link: both lift and drag are proportional to $\bar{q}$. Double your speed, quadruple your dynamic pressure — and thus quadruple the aerodynamic forces acting on the airframe. This is why structural failures tend to happen at high speed, and why stalls happen at low speed. Marqués & Da Ronch §3.1
Hold your hand flat outside a moving car. At 30 km/h, you feel a gentle upward push. At 100 km/h, the same flat hand gets pushed back much harder — that's drag increasing with speed. Tilt your hand up (increase the angle of attack) and it flies upward — that's lift. Tilt it too far and it becomes turbulent and just stalls backwards. Your hand just demonstrated every aerodynamic concept that governs a UAV wing.
$\rho$: air density [kg/m³]; $V$: true airspeed [m/s]; $S$: wing reference area [m²]; $C_L$: lift coefficient (dimensionless — depends on wing shape and angle of attack); $C_D$: drag coefficient; $m$: aircraft mass; $g$: gravity.
From the lift equation: for a fixed aircraft weight, flying at higher altitude (lower $\rho$) requires either higher speed $V$ or higher $C_L$ (fly at a higher angle of attack) to maintain the same lift. This is why high-altitude UAVs like the RQ-4 have enormous wings — at 20 km altitude, $\rho$ is only 7% of sea-level density, so you need a huge $S$ to generate enough lift at reasonable speeds. Sadraey §4.1
In reality, drag has two components: parasitic drag (always present, grows with $V^2$) and induced drag (caused by generating lift, gets worse at low speed). Together they form the drag polar: Marqués & Da Ronch §3.2
$C_{D0}$: zero-lift drag coefficient (typical fixed-wing UAV: 0.02–0.035); $e$: Oswald efficiency factor (0.75–0.90 for well-designed wings); $AR$: aspect ratio (wingspan² / wing area). High AR wings (like the RQ-4's $AR\approx25$) have very low $K$ — they produce lift efficiently with minimal induced drag. The MQ-9 has $AR \approx 20$, giving $K \approx 0.018$.
An aircraft in free flight can move in six independent ways — three translations (sliding forward/back, left/right, up/down) and three rotations (rolling, pitching, yawing). Engineers call these "degrees of freedom" or DOF. Every bump of turbulence, every gust of wind, every control input simultaneously affects all six. The 6-DOF equations of motion are the mathematical rules that describe all of this at once — and they're what autopilots and simulators are built on.
You might ask: why not just write the equations in the ground frame (North-East-Down)? The answer is the moment of inertia tensor $\mathbf{I}$. When a UAV pitches, rolls, and yaws, the distribution of mass relative to a ground-fixed axis changes constantly — making the equations impossibly complicated. In the body-fixed frame (axes attached to the aircraft), $\mathbf{I}$ is constant for a rigid body, and the equations become tractable. Marqués & Da Ronch §3.3
Newton's second law applied in the body frame, accounting for the fact that the frame itself is rotating (the Coriolis effect): Marqués & Da Ronch §3.4
What each variable means: $u, v, w$ = velocity components in body X, Y, Z (surge, sway, heave speeds). $p, q, r$ = angular rates — roll rate, pitch rate, yaw rate [rad/s]. $F_x, F_y, F_z$ = total external forces (aerodynamic + gravity + thrust) in body frame.
The Coriolis terms $(qw - rv)$ etc. arise because the body frame is rotating. Imagine sitting on a spinning office chair throwing a ball straight ahead — from the ground, the ball curves. From your perspective, there's a "fictitious force" curving it. These terms are that fictitious force for the aircraft body frame. They are responsible for the coupled motion you see in aircraft: a pitched-up, rolling aircraft yaws even with no rudder input.
The rotational counterpart — how angular velocity changes due to applied torques, with cross-coupling between axes: Marqués & Da Ronch §3.5
$I_{xx}, I_{yy}, I_{zz}$: moments of inertia about roll, pitch, yaw axes [kg·m²]. $L, M, N$: applied moments (roll, pitch, yaw) from aerodynamic forces and control surfaces.
The gyroscopic coupling terms $(I_{yy}-I_{zz})qr$ are critical. If you have a fast pitching rate $q$ and a fast yaw rate $r$, they generate a roll acceleration $\dot{p}$ even with no aileron input. This is the mathematical origin of dangerous coupled motions like spin entry, and why fly-by-wire UAVs need high-bandwidth roll/pitch/yaw controllers running simultaneously — not one at a time. Bestaoui Sebbane §2.2
An MQ-9 class UAV enters an evasive manoeuvre with pitch rate $q = 0.3$ rad/s and yaw rate $r = 0.2$ rad/s. Estimate the parasitic roll acceleration generated by gyroscopic coupling.
| Parameter | Value | Notes |
|---|---|---|
| $I_{xx}$ (roll) | 3,000 kg·m² | Mass concentrated in fuselage, narrow wingspan mass |
| $I_{yy}$ (pitch) | 45,000 kg·m² | Long fuselage + wing mass |
| $I_{zz}$ (yaw) | 47,000 kg·m² | Similar to pitch for conventional layout |
| $q$ | 0.3 rad/s | ≈ 17°/s pitch rate during pull-up |
| $r$ | 0.2 rad/s | ≈ 11°/s yaw rate during coordinated turn |
From Euler's roll equation with no applied roll moment ($L = 0$): $$I_{xx}\dot{p} = (I_{yy} - I_{zz})\,qr = (45000 - 47000) \times 0.3 \times 0.2 = -120 \text{ N·m}$$ $$\dot{p} = -120/3000 = \mathbf{-0.04 \text{ rad/s}^2}$$ The aircraft rolls at $-0.04$ rad/s² (about $2.3°/\text{s}^2$) with no aileron input at all. After just 2 seconds of combined pitch+yaw manoeuvre, the parasitic roll rate reaches $0.08$ rad/s. This is why autopilots on MALE UAVs must cross-feed pitch/yaw commands into the roll axis — failing to do so causes the aircraft to roll out of the intended flight path.
"Attitude" in aerospace means the orientation of the aircraft — which way is it pointing? The most intuitive way to describe it is with three angles: roll (how tilted sideways), pitch (nose up or down), and yaw (compass direction it's pointing). These are called Euler angles. The problem is that at certain extreme orientations, the math breaks down completely — a phenomenon called "gimbal lock." UAV autopilots use a more complex but reliable alternative called quaternions to avoid this.
Euler angles describe orientation as three sequential rotations: first yaw $\psi$ (rotate about vertical), then pitch $\theta$ (tilt nose up/down), then roll $\phi$ (tilt sideways). The standard aerospace convention is 3-2-1 (ZYX): Marqués & Da Ronch §3.6
Shorthand: $c_\phi = \cos\phi$, $s_\phi = \sin\phi$, etc. This matrix transforms a vector from the body frame to the NED (North-East-Down) ground frame. To go the other direction (NED → body), take the transpose: $R_{NED}^B = (R_B^{NED})^T$.
The Euler angle kinematic equations (how $\dot{\phi}, \dot{\theta}, \dot{\psi}$ relate to body rates $p, q, r$) are:
Spot the problem: At $\theta = \pm 90°$ (vertical climb or dive), $\tan\theta$ and $1/\cos\theta$ both become infinite. The equations blow up. Even for a standard fixed-wing UAV this matters — aggressive evasive manoeuvres or steep dive attacks routinely approach $\theta = 80°+$. Any autopilot using Euler angles will fail numerically in these conditions. Marqués & Da Ronch §3.7
Imagine a camera mounted on three rotating rings (a gimbal) to keep it level. When the middle ring rotates 90°, two of the outer rings suddenly align and act as one — you've lost a degree of freedom. Now you can't tilt the camera in one direction no matter how you turn the rings. This is gimbal lock. It was a real problem on the Apollo spacecraft and remains a real problem for any attitude system using Euler angles at extreme orientations.
A quaternion $\mathbf{q} = [q_0, q_1, q_2, q_3]^T$ encodes orientation as a single rotation of angle $\theta_r$ about an arbitrary unit axis $\hat{\mathbf{n}}$: Bestaoui Sebbane §2.3
Four numbers, one constraint — leaves 3 effective degrees of freedom, same as Euler angles, but no singularities anywhere. The quaternion kinematics are:
$$\dot{\mathbf{q}} = \frac{1}{2}\,\Xi(\mathbf{q})\,\boldsymbol{\omega}_B, \quad \Xi(\mathbf{q}) = \begin{bmatrix} -q_1 & -q_2 & -q_3 \\ q_0 & -q_3 & q_2 \\ q_3 & q_0 & -q_1 \\ -q_2 & q_1 & q_0 \end{bmatrix}$$This matrix never has a singularity. Every UAV autopilot (PX4, ArduPilot, military FCCs) uses quaternions internally for attitude tracking. Euler angles are only computed for display purposes — then converted back to quaternions before any control calculation.
The aerodynamic forces on a UAV — lift, drag, and side force, plus the rolling, pitching, and yawing moments — are all functions of how the aircraft is moving through the air. Rather than trying to model all this from first principles (complex CFD), engineers parameterise the forces using dimensionless coefficients that are measured in wind tunnels or computed. These coefficients, and how they change with small perturbations in speed or angle, are called stability derivatives — the "sensitivities" of the forces.
All aerodynamic forces and moments are expressed as dimensionless coefficients multiplied by dynamic pressure, reference area, and reference length: Sadraey §4.2
$\bar{q} = \frac{1}{2}\rho V^2$: dynamic pressure; $S$: reference wing area; $b$: wingspan (reference length for roll/yaw moments); $\bar{c}$: mean aerodynamic chord (reference length for pitch moment). $C_D, C_Y, C_L$: drag, side-force, lift coefficients. $C_l, C_m, C_n$: rolling, pitching, yawing moment coefficients.
Near a steady flight condition (trim), each coefficient is linearised as a function of small perturbations. The partial derivatives are the stability derivatives — the core aerodynamic data set for any UAV flight dynamics model: Marqués & Da Ronch §4.1
Term-by-term explanation:
$C_{m_0}$: pitching moment at zero angle of attack (trim offset).
$C_{m_\alpha}$: how much pitching moment changes per degree of angle of attack.
Must be negative for static pitch stability (if nose pitches up, a restoring nose-down moment is generated).
$C_{m_q}$: pitch damping derivative — pitching rapidly creates a restoring moment (like a weather vane slowing down).
$C_{m_{\dot\alpha}}$: lag derivative — effect of angle-of-attack rate (important for fast manoeuvres).
$C_{m_{\delta_e}}$: elevator effectiveness — how much moment per degree of elevator deflection.
| Derivative | Sign for Stability | Physical Meaning | Typical Range (MALE UAV) |
|---|---|---|---|
| $C_{m_\alpha}$ | Negative (<0) | Static pitch stability — nose-up generates nose-down moment | −0.5 to −2.0 /rad |
| $C_{l_\beta}$ | Negative (<0) | Dihedral effect — sideslip generates restoring roll | −0.05 to −0.2 /rad |
| $C_{n_\beta}$ | Positive (>0) | Weathercock stability — sideslip generates restoring yaw | +0.05 to +0.15 /rad |
| $C_{m_q}$ | Negative (<0) | Pitch damping — pitch rate is damped out | −5 to −20 /rad |
| $C_{l_p}$ | Negative (<0) | Roll damping — roll rate is damped out | −0.3 to −0.8 /rad |
| $C_{n_r}$ | Negative (<0) | Yaw damping — yaw rate is damped out | −0.05 to −0.3 /rad |
Even a statically stable aircraft doesn't just smoothly return to straight-and-level flight after a disturbance — it oscillates, like a pendulum. Some oscillations die out quickly (well-damped). Others persist for a long time (lightly damped). Some are fast and obvious; others are so slow a pilot might not even notice. These natural oscillatory patterns are called dynamic modes, and every fixed-wing aircraft has the same four of them. Understanding them is essential for designing autopilot controllers.
The linearised equations of motion (decoupled into longitudinal and lateral-directional sets) are solved as eigenvalue problems. The eigenvalues $\lambda = \sigma \pm j\omega$ give the natural frequencies and damping of each mode. Marqués & Da Ronch §5.1
| Mode | Motion Type | Typical Period | Damping | Military Significance |
|---|---|---|---|---|
| Short period | Fast pitch oscillation at roughly constant speed | 1–5 seconds | Well-damped (good) | Must be stable for pilot/autopilot handling; bandwidth limits missile-avoidance manoeuvres |
| Phugoid | Slow altitude/speed trade-off oscillation (nose up → slow → nose down → speed up → repeat) | 30–200 seconds | Lightly damped (tricky) | Phugoid instability causes altitude excursions that can bring UAV into terrain or threat envelopes |
| Dutch roll | Coupled rolling and yawing oscillation | 3–10 seconds | Lightly damped (often problematic) | Causes sensor-pointing jitter on ISR platforms; suppressed by yaw damper in FCC |
| Spiral mode | Slow diverging or converging bank angle change | Very slow (100s+ seconds) | Often mildly unstable | MALE UAVs often have unstable spiral — autopilot wing-leveller required for any unattended straight flight |
Lanchester's classical approximation for the phugoid natural frequency $\omega_{ph}$ depends only on trim speed, not on aerodynamic derivatives:
$$\omega_{ph} \approx \frac{g\sqrt{2}}{V_e} \quad [\text{rad/s}], \qquad T_{ph} = \frac{2\pi}{\omega_{ph}} \approx \frac{\pi V_e}{g\sqrt{2}/2}$$Why it matters: At MQ-9 cruise speed $V_e \approx 70$ m/s: $\omega_{ph} \approx 9.81\sqrt{2}/70 \approx 0.198$ rad/s, giving $T_{ph} \approx 31.7$ s. The phugoid period is very long — the aircraft slowly heaves up and down every 30 seconds. For a 27-hour ISR mission, an unstabilised phugoid means the aircraft's altitude drifts by hundreds of metres repeatedly. The altitude hold autopilot loop must have a bandwidth well below 0.2 rad/s to close around this mode without exciting it. Marqués & Da Ronch §5.2
"Trim" means the aircraft is in a steady, unaccelerated flight condition — all forces and moments are exactly balanced so nothing is changing. Before you can design an autopilot, you must know the trim condition: what speed, what angle of attack, what elevator deflection, what throttle setting produce straight-and-level flight at the target altitude and airspeed? Finding trim is a nonlinear root-finding problem — you're solving for the combination of inputs that makes all six equations of motion equal to zero simultaneously.
Trim is defined by setting all accelerations and angular rates to zero in the 6-DOF equations. For steady wings-level flight at speed $V_e$ and altitude $h$: Marqués & Da Ronch §6.1
Interpretation: The trim lift coefficient is set by weight and speed (we need exactly enough lift). The trim elevator deflection is whatever is needed to make pitching moment $C_m = 0$. Both depend on $\alpha_{trim}$ — the angle of attack at which the wing produces the required lift. The trim algorithm solves these simultaneously.
Find the trim angle of attack and elevator deflection for the MQ-9 at $V_e = 70$ m/s, $h = 7620$ m, using representative aerodynamic coefficients.
| Aerodynamic Parameter | Value | Source |
|---|---|---|
| $C_{L_0}$ | 0.10 | Estimated (camber effect at zero AoA) |
| $C_{L_\alpha}$ | 5.5 /rad | Estimated from AR=20, $e$=0.85 |
| $C_{m_0}$ | 0.04 | Estimated (slight nose-down at zero AoA) |
| $C_{m_\alpha}$ | −0.85 /rad | Stable aircraft; cg aft of aerodynamic center |
| $C_{m_{\delta_e}}$ | −1.2 /rad | Elevator effectiveness |
| Wing loading $W/S$ | $4760\times9.81/16.84 = 2773$ N/m² | From published specs |
| $\rho$ at 7620 m | 0.549 kg/m³ | ISA table |
Step 1 — Required lift coefficient: $$C_{L,trim} = \frac{2W/S}{\rho V_e^2} = \frac{2\times2773}{0.549\times70^2} = \frac{5546}{2690.1} = \mathbf{0.661}$$ Step 2 — Trim angle of attack: $$\alpha_{trim} = \frac{C_{L,trim} - C_{L_0}}{C_{L_\alpha}} = \frac{0.661 - 0.10}{5.5} = \frac{0.561}{5.5} = \mathbf{0.102 \text{ rad} \approx 5.8°}$$ Step 3 — Trim elevator deflection: $$\delta_{e,trim} = \frac{-C_{m_0} - C_{m_\alpha}\alpha_{trim}}{C_{m_{\delta_e}}} = \frac{-0.04 - (-0.85)(0.102)}{-1.2} = \frac{-0.04 + 0.0867}{-1.2} = \frac{0.0467}{-1.2} = \mathbf{-0.039 \text{ rad} \approx -2.2°}$$ Interpretation: The MQ-9 at 25,000 ft cruise needs about 5.8° nose-up angle of attack (typical for efficient cruise) and about 2.2° elevator trailing-edge-up deflection to maintain level flight. The elevator deflection is small — well within the ±20° range — indicating a well-designed aircraft in its intended operating envelope.
A quadrotor has four propellers and no control surfaces. It manoeuvres entirely by changing how fast each rotor spins — spin some faster, others slower. This is much simpler mechanically than a helicopter with collective pitch and swashplates, but the control problem is tricky: you need to control 6 degrees of freedom with only 4 independent actuators. The good news is that three of the six (altitude, roll, pitch) are directly controllable. The bad news is that the platform is inherently unstable — without active control updating at 100+ Hz, it flips over in about 0.1 seconds.
Each rotor $i$ spinning at rate $\Omega_i$ [rad/s] produces thrust $T_i$ upward and a reaction torque $\tau_i$ opposing rotation. These are modelled as: Bestaoui Sebbane §2.4
$k_T$: thrust coefficient [N·s²/rad²] — depends on rotor geometry and blade pitch. $k_M$: torque coefficient [N·m·s²/rad²] — typically $k_M/k_T \approx 0.01$–$0.02$. Both scale with $\Omega^2$ — doubling rotor speed quadruples thrust. This nonlinear relationship means the control inputs (desired thrust/torque) must be square-rooted to get the commanded motor speeds. This is the "control allocation" problem.
In an X-configuration quadrotor, opposite rotors spin in the same direction. The full mapping from rotor speeds to vehicle forces and moments is: Bestaoui Sebbane §2.5
$l$: arm length from centre to rotor [m]. Row 1: total thrust (all rotors contribute equally). Rows 2–3: roll and pitch moments (differential thrust between opposing pairs). Row 4: yaw moment (differential reaction torque — clockwise vs counter-clockwise pairs).
Inversion: To find the rotor speeds needed for a desired $(T, L, M, N)$: $\boldsymbol{\Omega}^2 = \mathbf{B}^{-1}(T, L, M, N)^T$. Then take element-wise square roots. If any $\Omega_i^2 < 0$, the demanded force/moment combination is physically impossible for this platform — it cannot be achieved.
Rotor 1 ($\Omega_1 = 0$) fails completely. What force/moment combinations are still achievable? This matters for military UAVs operating in environments where rotors are vulnerable to hits.
With $\Omega_1 = 0$, the control matrix effectively loses its first column. The remaining three rotors $(\Omega_2, \Omega_3, \Omega_4)$ provide only three independent inputs for four outputs — the system is now under-actuated.
What's lost: The ability to independently control roll, pitch, yaw, and altitude simultaneously. Specifically: with $\Omega_1 = 0$, generating roll left (reducing rotor 4, increasing rotor 2) simultaneously changes the yaw moment in a fixed relationship. Roll and yaw become coupled — you cannot change one without changing the other.
What remains possible: Research (notably Mueller & D'Andrea 2014) shows that a quadrotor can still land safely with one rotor failed if it accepts continuous spinning about the yaw axis (body spin). The vehicle descends while spinning — controllable altitude and lateral position, but not heading. This is implemented in PX4 as the "motor failure landing" mode. Bestaoui Sebbane §2.6
At hover (zero translation, zero angular rate), the trim condition requires each rotor to produce exactly one-quarter of the aircraft weight:
For a 1.5 kg quadrotor with $k_T = 2.9\times10^{-5}$ N·s²/rad²: $\Omega_{hover} = \sqrt{1.5\times9.81/(4\times2.9\times10^{-5})} \approx 356$ rad/s $\approx 3,400$ RPM. The linearised hover dynamics show that the translational modes are unstable (a displaced quadrotor accelerates away from its position unless actively corrected) — hence the need for position hold loops running at 100+ Hz.
pip install numpy scipy matplotlib then python quadrotor_6dof.py
# quadrotor_6dof.py # ───────────────────────────────────────────────────────────────── # Full 6-DOF quadrotor flight dynamics simulator. # Implements: Bestaoui Sebbane §2.4–2.6, Marqués & Da Ronch §3.3–3.5 # Attitude representation: unit quaternions (singularity-free) # Integrator: scipy RK45 (adaptive step-size) # ───────────────────────────────────────────────────────────────── import numpy as np from scipy.integrate import solve_ivp from dataclasses import dataclass # ── Physical Parameters ────────────────────────────────────────── @dataclass class QuadrotorParams: """ Physical parameters for a small quadrotor UAV. Representative of a military Group 1 platform. """ m: float = 1.5 # mass [kg] g: float = 9.80665 # gravity [m/s²] Ixx: float = 0.0348 # roll moment of inertia [kg·m²] Iyy: float = 0.0348 # pitch moment of inertia [kg·m²] Izz: float = 0.0717 # yaw moment of inertia [kg·m²] k_T: float = 2.9e-5 # thrust coefficient [N·s²/rad²] k_M: float = 5.0e-7 # torque coefficient [N·m·s²/rad²] l_arm: float = 0.23 # arm length (centre to rotor) [m] def control_allocation_matrix(p: QuadrotorParams) -> np.ndarray: """ Build 4×4 control allocation matrix B. Maps [Ω1², Ω2², Ω3², Ω4²] → [Thrust, Roll moment, Pitch moment, Yaw moment]. X-configuration: rotors 1,3 CW; rotors 2,4 CCW. Bestaoui Sebbane §2.5 """ kT, kM, l = p.k_T, p.k_M, p.l_arm B = np.array([ [ kT, kT, kT, kT ], # total thrust [-kT*l, kT*l, kT*l, -kT*l ], # roll moment (L) [-kT*l, kT*l, -kT*l, kT*l ], # pitch moment (M) [-kM, -kM, kM, kM ], # yaw moment (N) ]) return B def hover_trim(p: QuadrotorParams) -> np.ndarray: """ Compute hover trim rotor speeds (all equal). From: 4*k_T*Ω² = m*g → Ω = sqrt(m*g / (4*k_T)) """ omega_sq_each = p.m * p.g / (4 * p.k_T) return np.sqrt(omega_sq_each) * np.ones(4) # [rad/s] def quat_kinematics(q: np.ndarray, omega: np.ndarray) -> np.ndarray: """ Quaternion kinematics: dq/dt = 0.5 * Xi(q) * omega Marqués & Da Ronch §3.7 — singularity-free attitude propagation. q : unit quaternion [q0, q1, q2, q3] omega : body angular velocity [p, q, r] rad/s """ q0, q1, q2, q3 = q p_r, q_r, r_r = omega # Xi matrix (4×3) Xi = np.array([ [-q1, -q2, -q3], [ q0, -q3, q2], [ q3, q0, -q1], [-q2, q1, q0], ]) return 0.5 * Xi @ omega def quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray: """Convert unit quaternion to 3×3 rotation matrix (body → world).""" q0, q1, q2, q3 = q return np.array([ [1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)], [2*(q1*q2+q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], [2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1**2+q2**2)], ]) def quadrotor_ode(t, state, rotor_speeds, params): """ 6-DOF quadrotor equations of motion in body frame. State vector (13 elements): [0:3] = position (x, y, z) in world NED frame [m] [3:7] = quaternion (q0, q1, q2, q3) — body → world rotation [7:10] = velocity (vx, vy, vz) in world frame [m/s] [10:13]= angular velocity (p, q, r) in body frame [rad/s] Marqués & Da Ronch §3.4–3.5, Bestaoui Sebbane §2.4 """ p_state = params pos = state[0:3] # position in world q_att = state[3:7] # quaternion vel_w = state[7:10] # velocity in world omega = state[10:13] # angular velocity in body [p, q, r] # ── Control allocation ─────────────────────────────────────── B = control_allocation_matrix(p_state) omega2 = rotor_speeds**2 # square of each rotor speed forces = B @ omega2 # [Thrust, L, M, N] T_total, L_mom, M_mom, N_mom = forces # ── Rotation matrix body → world ───────────────────────────── R = quat_to_rotation_matrix(q_att) # ── Translational dynamics (world frame) ───────────────────── # Thrust vector acts along body z-axis (upward in body = -z in NED) thrust_world = R @ np.array([0, 0, T_total]) # rotate to world gravity_world = np.array([0, 0, p_state.m * p_state.g]) # downward d_pos = vel_w d_vel = (thrust_world - gravity_world) / p_state.m # ── Quaternion kinematics ──────────────────────────────────── d_q = quat_kinematics(q_att, omega) # ── Rotational dynamics — Euler's equations (body frame) ───── # Marqués & Da Ronch §3.5 p_r, q_r, r_r = omega Ixx, Iyy, Izz = p_state.Ixx, p_state.Iyy, p_state.Izz dp = (L_mom + (Iyy - Izz) * q_r * r_r) / Ixx # roll accel dq = (M_mom + (Izz - Ixx) * p_r * r_r) / Iyy # pitch accel dr = (N_mom + (Ixx - Iyy) * p_r * q_r) / Izz # yaw accel # Quaternion renormalisation correction (prevents drift) q_norm_err = np.dot(q_att, q_att) - 1.0 d_q -= 0.5 * q_norm_err * q_att return np.concatenate([d_pos, d_q, d_vel, [dp, dq, dr]]) def run_simulation(params: QuadrotorParams, t_end=5.0, perturb=False): """ Simulate quadrotor from hover trim for t_end seconds. If perturb=True, adds a small initial roll angle to demonstrate instability. """ # Initial state: hovering at origin, level q0_init = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion = level if perturb: # 5° roll perturbation to show open-loop instability phi_perturb = np.radians(5.0) q0_init = np.array([np.cos(phi_perturb/2), np.sin(phi_perturb/2), 0, 0]) state0 = np.concatenate([ [0.0, 0.0, 0.0], # initial position [m] q0_init, # initial quaternion [0.0, 0.0, 0.0], # initial velocity [m/s] [0.0, 0.0, 0.0], # initial angular velocity [rad/s] ]) omega_trim = hover_trim(params) print(f"Hover trim rotor speed: {omega_trim[0]:.1f} rad/s " f"({omega_trim[0]*60/(2*np.pi):.0f} RPM)") sol = solve_ivp( quadrotor_ode, [0, t_end], state0, args=(omega_trim, params), method='RK45', max_step=0.01, # 100 Hz max — matches flight controller rate rtol=1e-6, atol=1e-8, dense_output=True, ) # Extract results pos_final = sol.y[0:3, -1] q_final = sol.y[3:7, -1] q_norm = np.linalg.norm(q_final) # Convert final quaternion back to Euler angles for human-readable output q0, q1, q2, q3 = q_final / q_norm phi_f = np.degrees(np.arctan2(2*(q0*q1+q2*q3), 1-2*(q1**2+q2**2))) theta_f = np.degrees(np.arcsin(2*(q0*q2-q3*q1))) psi_f = np.degrees(np.arctan2(2*(q0*q3+q1*q2), 1-2*(q2**2+q3**2))) print(f"Simulation: {'PERTURBED (5° roll)' if perturb else 'NOMINAL HOVER'}") print(f" Duration: {t_end} s ({len(sol.t)} solver steps)") print(f" Final position: x={pos_final[0]:.4f}, y={pos_final[1]:.4f}, z={pos_final[2]:.4f} m") print(f" Final attitude: φ={phi_f:.3f}°, θ={theta_f:.3f}°, ψ={psi_f:.3f}°") print(f" Quaternion norm: {q_norm:.8f} (should be 1.0)") print(f" Altitude drift: {abs(pos_final[2]):.4f} m (should be ≈ 0)") return sol if __name__ == "__main__": p = QuadrotorParams() print("\n" + "="*52) print(" QUADROTOR 6-DOF SIMULATION — HOVER VALIDATION") print("="*52) # Test 1: Perfect hover — should stay at origin print("\n[TEST 1 — Perfect Hover Trim]") sol1 = run_simulation(p, t_end=5.0, perturb=False) # Test 2: Perturbed hover — shows open-loop instability print("\n[TEST 2 — 5° Roll Perturbation, No Controller]") sol2 = run_simulation(p, t_end=2.0, perturb=True) print("\n[GYROSCOPIC COUPLING CHECK — Euler's Equations]") p_rate = 0.3 # pitch rate [rad/s] r_rate = 0.2 # yaw rate [rad/s] roll_accel = (p.Iyy - p.Izz) * p_rate * r_rate / p.Ixx print(f" With q={p_rate} rad/s, r={r_rate} rad/s (no applied roll moment):") print(f" Parasitic roll acceleration = {roll_accel:.4f} rad/s² ({np.degrees(roll_accel):.2f} °/s²)") print(f" Roll rate after 2s = {roll_accel*2:.3f} rad/s ({np.degrees(roll_accel*2):.1f} °/s)") print(f" → This is why pitch+yaw manoeuvres MUST cross-feed the roll axis.")