Skip to content

Quadrotor / multirotor UAV — Nonlinear 6-DoF dynamics

Rigid-body quadrotor model in the full 6-DoF formulation: 12 states (position, velocity, attitude, angular rates), 4 control inputs (collective thrust + three body-frame torques). Implemented in pure Python/NumPy with Euler and RK4 integrators. Suitable for PID stabilisation, MPC trajectory tracking, and adaptive RL critics (iADP, AIDI, AA-INDI) — especially for rotor-failure scenarios.

Quadrotor X-configuration: top-down view showing body-frame axes, motor numbering M1–M4, rotation directions CCW/CW, per-motor thrusts \(f_i\), body torques \(\tau_x, \tau_y, \tau_z\), and the collective thrust \(T\)

  • Quick start

    Run a 10-second hover in three lines.

    To example

  • Model API

    NonlinearQuadrotor — the 6-DoF dynamics class.

    To API

  • Mathematics

    Equations of motion, NED / body frame conventions.

    To theory

  • Validation tests

    Hover, free fall, gyroscopic coupling.

    To checks

Reference platform parameters

The defaults match a typical 1.5 kg research quadrotor (AscTec Hummingbird / Pelican class), arm length 22.5 cm. These are the defaults from QuadrotorParameters.default_parameters(); override them for other platforms (DJI F450, Crazyflie, X4-frame, etc.).

Parameter Value
Mass \(m\), kg 1.5
Inertia \(J_x = J_y\), kg·m² 0.0211
Inertia \(J_z\), kg·m² 0.0366
Arm length, m 0.225
Linear body-drag \(k_{dx}=k_{dy}\), N·s/m 0.10
Linear body-drag \(k_{dz}\), N·s/m 0.20
Maximum collective thrust, N 30 (≈ 2:1 thrust-to-weight)
Maximum torque per axis, N·m 1.5

Coordinate systems and state

The model uses two right-handed orthogonal frames:

  • Body-fixed: \(x\) forward, \(y\) right, \(z\) down — coincides with NED when level.
  • Earth (NED, north-east-down): \(x\) north, \(y\) east, \(z\) down. Gravity is along \(+z\).

Body-to-earth transformation is the standard ZYX (321) Euler rotation matrix (yaw → pitch → roll).

State vector and control:

\[ \mathbf{x} = \begin{bmatrix} x_e & y_e & z_e & u_b & v_b & w_b & \phi & \theta & \psi & p & q & r \end{bmatrix}^\top, \qquad \mathbf{u} = \begin{bmatrix} T & \tau_x & \tau_y & \tau_z \end{bmatrix}^\top. \]
  • \(x_e, y_e, z_e\) — earth-frame position (NED), m.
  • \(u_b, v_b, w_b\) — body-frame velocity, m/s.
  • \(\phi, \theta, \psi\) — roll, pitch, yaw (ZYX Euler), rad.
  • \(p, q, r\) — body-frame angular rates, rad/s.
  • \(T\) — collective thrust along body \(-z\) (up when level), N.
  • \(\tau_x, \tau_y, \tau_z\) — body-frame torques (roll, pitch, yaw), N·m.

The control vector \(\mathbf{u}\) is already the output of the motor-mixing / allocation block. Mapping the 4 rotor speeds \(\omega_i\) to \((T, \tau)\) depends on the configuration (X / +) and is not part of this model — it lives in a separate helper or inside the RL agent.

Euler-angle singularity

At \(|\theta| = \pi/2\) the kinematic mapping diverges through the \(1/\cos\theta\) term (gimbal lock). Use a quaternion variant for aggressive acrobatic manoeuvres — currently not implemented.

Mathematical model

The combined ODE has four blocks: position kinematics, velocity dynamics, Euler-angle kinematics, angular-rate dynamics.

1. Position kinematics

\[ \dot{\mathbf{r}}_e = R_{eb}(\phi,\theta,\psi)\,\mathbf{v}_b, \]

where \(R_{eb}\) is the body-to-earth rotation (ZYX 321):

\[ R_{eb} = \begin{bmatrix} c_\theta c_\psi & s_\phi s_\theta c_\psi - c_\phi s_\psi & c_\phi s_\theta c_\psi + s_\phi s_\psi \\ c_\theta s_\psi & s_\phi s_\theta s_\psi + c_\phi c_\psi & c_\phi s_\theta s_\psi - s_\phi c_\psi \\ -s_\theta & s_\phi c_\theta & c_\phi c_\theta \end{bmatrix} \]

(shorthand \(c_\bullet = \cos\bullet\), \(s_\bullet = \sin\bullet\)).

2. Velocity dynamics (body frame)

Newton's equation in the body frame, including rotation, gravity, thrust, and linear aerodynamic drag:

\[ m\,\dot{\mathbf{v}}_b \;=\; R_{be}\,\mathbf{F}_{\text{grav},e} \;+\; \mathbf{F}_{\text{thrust},b} \;-\; D\,\mathbf{v}_b \;-\; \boldsymbol\omega \times m\,\mathbf{v}_b, \]

with:

  • \(\mathbf{F}_{\text{grav},e} = [0, 0, m g]^\top\) — gravity in NED (z-down),
  • \(\mathbf{F}_{\text{thrust},b} = [0, 0, -T]^\top\) — thrust along body \(-z\),
  • \(D = \mathrm{diag}(k_{dx}, k_{dy}, k_{dz})\) — diagonal drag matrix,
  • \(\boldsymbol\omega = [p, q, r]^\top\).

3. Euler-angle kinematics (ZYX 321)

\[ \begin{bmatrix} \dot\phi \\ \dot\theta \\ \dot\psi \end{bmatrix} = \begin{bmatrix} 1 & s_\phi t_\theta & c_\phi t_\theta \\ 0 & c_\phi & -s_\phi \\ 0 & s_\phi / c_\theta & c_\phi / c_\theta \end{bmatrix} \begin{bmatrix} p \\ q \\ r \end{bmatrix}, \]

with \(t_\theta = \tan\theta\).

4. Angular-rate dynamics (Newton-Euler)

With diagonal inertia \(\mathbf{J} = \mathrm{diag}(J_x, J_y, J_z)\):

\[ \boxed{\; \begin{aligned} \dot p &= \bigl(\tau_x + (J_y - J_z)\,q\,r\bigr) / J_x, \\ \dot q &= \bigl(\tau_y + (J_z - J_x)\,p\,r\bigr) / J_y, \\ \dot r &= \bigl(\tau_z + (J_x - J_y)\,p\,q\bigr) / J_z. \end{aligned} \;} \]

The \((J_i - J_j)\,\omega_i\,\omega_j\) terms are the gyroscopic cross-axis coupling (Euler cross-product terms).

Quick start

import numpy as np

from tensoraerospace.aerospacemodel.quadrotor.nonlinear import NonlinearQuadrotor

# Hover at the origin
m = NonlinearQuadrotor(
    x0=np.zeros(12),
    dt=0.01,
    integrator="rk4",
)

# T = m·g holds the vehicle in equilibrium
u_hover = np.array([m.hover_thrust, 0.0, 0.0, 0.0])

for _ in range(1000):  # 10 seconds
    m.run_step(u_hover)

print(f"Final state (max |x|): {np.max(np.abs(m.current_state)):.2e}")
# → ≈ 0 (exact equilibrium with RK4)

Hover with a perturbation and a small roll-torque

import numpy as np

from tensoraerospace.aerospacemodel.quadrotor.nonlinear import (
    NonlinearQuadrotor,
    set_initial_state,
)

# Start with a slight 3-degree roll
m = NonlinearQuadrotor(
    x0=set_initial_state(phi=np.deg2rad(3.0)),
    dt=0.005,
    integrator="rk4",
)

# Hover thrust + small negative roll command (P-feedback)
T_hover = m.hover_thrust
for k in range(2000):
    tau_x = -0.05 * m.current_state[6]   # proportional feedback on phi
    u = np.array([T_hover, tau_x, 0.0, 0.0])
    m.run_step(u)

phi_final = np.rad2deg(m.current_state[6])
print(f"Final roll angle: {phi_final:.4f}°")

Initial-state helper

from tensoraerospace.aerospacemodel.quadrotor.nonlinear import set_initial_state

# Start: 5 m altitude (NED z = -5), 5° pitch
x0 = set_initial_state(z_e=-5.0, theta=np.deg2rad(5.0))

Validation

ODE correctness is verified by five analytical tests in tests/aerospacemodel/quadrotor_test.py:

Test Check Tolerance
Hover equilibrium \(T = m\,g\), zero torque → state unchanged over 10 s \(\max\|x\| < 10^{-9}\)
Free fall, no drag \(z_e(t) = \tfrac{1}{2}g\,t^2\) \(\delta < 10^{-3}\)
Free fall with drag \(z(t) = v_\infty t + v_\infty\tau(e^{-t/\tau} - 1)\) \(\delta < 10^{-3}\)
Single-step roll-torque \(p \approx \tau_x \cdot dt / J_x\) \(\delta < 10^{-9}\)
Gyroscopic coupling \(\dot p = (J_y-J_z)\,q\,r / J_x\) at \(q=r=1\) \(\delta < 10^{-12}\)

Plus 6 sanity checks: input dimensions, history accumulation, both integrator backends (Euler + RK4), invalid-integrator-name handling.

Allocation (mixer): bridge between virtual control and rotor speeds

The bare NonlinearQuadrotor model takes the virtual vector \((T, \tau_x, \tau_y, \tau_z)\) — the level at which PID/MPC/RL controllers naturally operate. To reproduce rotor-level failures (needed for the FTC scenarios in Lu 2019, Wang 2019, Lanzon 2015), a bidirectional XConfigAllocator is provided:

\[ \begin{bmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix} = \underbrace{ \begin{bmatrix} k_T & k_T & k_T & k_T \\ -k_T a & k_T a& k_T a& -k_T a \\ k_T a & -k_T a& k_T a& -k_T a \\ k_M & k_M & -k_M & -k_M \end{bmatrix} }_{M\ \text{(X-config, PX4)}} \begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix} \]

with \(a = L/\sqrt 2\), \(L\) the arm length, \(k_T\) the thrust coefficient, and \(k_M\) the yaw-torque coefficient. The matrix is full-rank, so the inverse is well-defined.

from tensoraerospace.aerospacemodel.quadrotor import default_allocator

alloc = default_allocator()  # k_T=7.5e-6, k_M≈0.016·k_T, arm=0.225
v = alloc.mix(omega_squared)         # 4 ω² → [T, τ]
omega2 = alloc.unmix(v)              # [T, τ] → 4 ω² (may be < 0 for non-realisable v)
omega2 = alloc.saturate(omega2, 0, 1000)  # clip to physical bounds

The NonlinearQuadrotor-v0 env supports two action modes:

# Mode "virtual" (default): action = [T, τ_x, τ_y, τ_z]
env = gym.make("NonlinearQuadrotor-v0", initial_state=np.zeros(12),
               number_time_steps=1000, action_space="virtual")

# Mode "rotor": action = [ω₁², ω₂², ω₃², ω₄²]; env applies allocator internally
env = gym.make("NonlinearQuadrotor-v0", initial_state=np.zeros(12),
               number_time_steps=1000, action_space="rotor")

Without damage_profile both modes are equivalent (round-trip mix↔unmix).

Damage subsystem (rotor-level events)

To reproduce the canonical FTC scenarios from the literature, the env ships with an event-driven rotor-level failure system. Each rotor has an effectiveness coefficient \(\mu_i \in [0, 1]\), and the env applies \(\omega^2_{i,\text{eff}} = \mu_i \cdot \omega^2_{i,\text{cmd}}\) before mixing back to \((T, \tau)\).

Three event types:

Event Semantics Source
RotorDamageEvent(rotor_id, mu) Instantaneous effectiveness loss on rotor \(i\) Lu et al. 2019
RotorLossEvent(rotor_id) Complete failure (\(\mu = 0\)) Lanzon et al. 2015
MotorEfficiencyDecay(rotor_id, tau, mu_floor) Exponential wear \(\dot\mu = -(1/\tau)(\mu - \mu_\text{floor})\) gradual wear

Three ready-made presets: LANZON_M1_LOSS, LU_M1_50PCT_LOSS, WEAR_DEGRADATION_M3. Import from tensoraerospace.aerospacemodel.quadrotor.damage.

from tensoraerospace.aerospacemodel.quadrotor.damage import LANZON_M1_LOSS

env = gym.make("NonlinearQuadrotor-v0", initial_state=np.zeros(12),
               number_time_steps=2000, damage_profile=LANZON_M1_LOSS)
obs, _ = env.reset()
T_hover = 1.5 * 9.81
for k in range(2000):
    obs, r, term, trunc, info = env.step(np.array([T_hover, 0, 0, 0]))
    if "damage_events_triggered" in info:
        print(f"t={k*0.01}s: {info['damage_events_triggered']}")

Without damage_profile the env is bit-identical to the no-damage baseline (rotor-effectiveness \(\mu = 1\) on all four motors).

Current limitations

  1. ZYX 321 Euler angles — the model has gimbal lock at \(|\theta| = \pi/2\). Acrobatic manoeuvres (loops, flips) need a quaternion variant — future extension.
  2. Linear drag only — no quadratic-in-velocity term (significant above ~10 m/s cruise) and no blade-flap aerodynamics.
  3. Symmetric X-frame in the allocator — asymmetric frames (Y, H, hex/octocopter) need their own allocator class.
  4. Element-wise saturation only — no thrust-priority allocation (Faessler 2017) for control-saturation regimes where one channel should be preserved over another.

References

Dynamics and coordinate systems

  • Stevens, B. L., Lewis, F. L., Johnson, E. N. (2015). Aircraft Control and Simulation, 3rd ed. Wiley. — methodological foundation for the 6-DoF rigid-body equations in the body-fixed frame (NED, ZYX Euler convention).
  • PX4 Airframe Reference: Quadrotor X — motor numbering and rotation-direction convention used in XConfigAllocator.

Reference platform parameters

  • AscTec Hummingbird / Pelican research-class quadrotors — typical values \(m \approx 1.5\) kg, \(J_x = J_y \approx 0.021\) kg·m², \(J_z \approx 0.037\) kg·m², 22.5 cm arm. Widely used in academic UAV FTC papers.

FTC scenarios (sources for the damage subsystem and presets)

  • Lu, P., Yu, B., van Kampen, E.-J., Chu, Q. P. (2019). Quadrotor Fault Tolerant Incremental Sliding Mode Control driven by Sliding Mode Disturbance Observers. Aerospace Science and Technology, 87:417–430. DOI: 10.1016/j.ast.2019.03.001 — multiplicative rotor effectiveness loss; basis for the LU_M1_50PCT_LOSS preset and the RotorDamageEvent class. 129 citations.
  • Wang, X., van Kampen, E.-J., Chu, Q. P. (2019). Quadrotor fault-tolerant incremental nonsingular terminal sliding mode control. Aerospace Science and Technology, 95:105514. DOI: 10.1016/j.ast.2019.105514 — parallel work using terminal sliding mode; same fault model.
  • Lanzon, A., Freddi, A., Longhi, S. (2015). Active fault-tolerant control for quadrotors subjected to a complete rotor failure. IEEE/RSJ IROS 2015. DOI: 10.1109/IROS.2015.7354046 — extreme case of complete rotor failure (spin-mode recovery); basis for LANZON_M1_LOSS and the RotorLossEvent class.
  • Aircraft damage modeling (F-16) — a more developed damage subsystem for fixed-wing aircraft, with strip-theory and Huygens-Steiner inertia recompute.
  • AIDI — adaptive INDI-style architecture applicable to this quadrotor model for fault-tolerant control.
  • iADP, IM-GDHP — online adaptive critics that recover from plant changes within tens of milliseconds.

Python API

NonlinearQuadrotor(x0, selected_state_output=None, t0=0, dt=0.01, integrator='rk4')

Bases: ModelBase

Pure-numpy 6-DoF rigid-body quadrotor in NED frame.

Action: [T, tau_x, tau_y, tau_z] — collective thrust (N) and body-frame torques (N·m). The model is "abstract" in the actuation: motor mixing / RPM allocation is not part of this class. Use the helper in utils.py (TODO) or the env layer for that.

State: see module docstring of __init__.py for the 12-element layout.

current_state property

Most recent state as a flat 1-D ndarray (12 elements).

hover_thrust property

Collective thrust that holds the vehicle stationary at level attitude.

QuadrotorParameters(m=1.5, g=9.81, Jx=0.0211, Jy=0.0211, Jz=0.0366, arm_length=0.225, kdx=0.1, kdy=0.1, kdz=0.2, thrust_max=30.0, thrust_min=0.0, torque_max=1.5) dataclass

Inertial + geometric parameters of a single rigid-body quadrotor.

quadrotor_ode_6dof(x, u, t, params)

Right-hand side of the 6-DoF quadrotor ODE.

Parameters:

Name Type Description Default
x ndarray

state vector (12,) — see module docstring for layout.

 required
u ndarray

control vector (4,) — [T, tau_x, tau_y, tau_z].

 required
t float

current time, s (unused — model is autonomous).

 required
params QuadrotorParameters

inertial / geometric parameters.

 required

Returns:

Type Description
ndarray

dx/dt of shape (12,).

XConfigAllocator(k_T=7.5e-06, k_M=1.2e-07, arm_length=0.225) dataclass

Bidirectional X-quad rotor↔virtual allocator.

The allocation matrix :math:M is defined by

.. math::

\begin{bmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix}
= M \begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix},

where (with :math:a = L/\sqrt 2)

.. math::

M = \begin{bmatrix}
      k_T   &  k_T  &  k_T  &  k_T   \\
     -k_T a &  k_T a&  k_T a& -k_T a \\
      k_T a & -k_T a&  k_T a& -k_T a \\
      k_M   &  k_M  & -k_M  & -k_M
    \end{bmatrix}.

The matrix is full-rank for non-degenerate parameters, so the inverse :func:unmix is well-defined.

Parameters:

Name Type Description Default
k_T float

thrust coefficient (N per :math:(\mathrm{rad/s})^2). Default value matches a typical 1.5 kg quad with :math:\omega_\max \approx 1000\,\mathrm{rad/s} and per-motor max thrust :math:\approx 7.5\,\mathrm{N}.

 7.5e-06
k_M float

yaw-torque coefficient (N·m per :math:(\mathrm{rad/s})^2). Typically k_M / k_T ≈ 0.016 m for small props.

 1.2e-07
arm_length float

distance from CG to each rotor, m.

 0.225

matrix property

Read-only 4×4 forward allocation matrix M.

matrix_inverse property

Read-only 4×4 inverse matrix M⁻¹.

mix(omega_squared)

Map :math:(\omega_1^2, \ldots, \omega_4^2) → :math:(T, \tau_x, \tau_y, \tau_z).

Parameters:

Name Type Description Default
omega_squared ndarray

shape (4,), units :math:(\mathrm{rad/s})^2. Must be non-negative element-wise.

 required

Returns:

Type Description
ndarray

Virtual control vector [T, τ_x, τ_y, τ_z] of shape (4,).

unmix(virtual)

Map :math:(T, \tau_x, \tau_y, \tau_z) → :math:(\omega_1^2, \ldots, \omega_4^2).

Note: the result may contain negative entries when the commanded virtual triple is not realisable by 4 unidirectional rotors (e.g. requesting :math:T = 0 with non-zero pitch torque). The caller should pass the result through :func:saturate before feeding it into :func:mix.

Parameters:

Name Type Description Default
virtual ndarray

shape (4,), units N (T) and N·m (τ).

 required

Returns:

Type Description
ndarray

Rotor speeds-squared [ω₁², ω₂², ω₃², ω₄²] of shape (4,).
ndarray

Possibly negative — pass through :func:saturate if needed.

saturate(omega_squared, omega_min=0.0, omega_max=1000.0)

Clip rotor speeds-squared into :math:[\omega_\min^2, \omega_\max^2].

Element-wise clip — does not preserve any virtual quantity. For thrust-priority allocation strategies, see the literature (e.g. Faessler 2017); this method is the simplest sane default.

Parameters:

Name Type Description Default
omega_squared ndarray

shape (4,), possibly out-of-bounds.

 required
omega_min float

lower rotor-speed bound, rad/s. Default 0 (perpetual zero allowed — useful for rotor-loss scenarios). Must be non-negative.

 0.0
omega_max float

upper rotor-speed bound, rad/s. Default 1000 (≈ 9550 RPM).

 1000.0

Returns:

Type Description
ndarray

Clipped [ω₁², ω₂², ω₃², ω₄²].

RotorDamageEvent(trigger_time, rotor_id, label=None, mu=1.0) dataclass

Bases: DamageEvent

Instantaneous multiplicative effectiveness loss on rotor rotor_id.

mu is the surviving fraction (1.0 = healthy, 0.0 = total loss). Mirrors the convention in Lu 2019 (ISMC FTC): :math:\omega^2_{i,\text{eff}} = \mu \cdot \omega^2_{i,\text{cmd}}.

RotorLossEvent(trigger_time, rotor_id, label=None) dataclass

Bases: DamageEvent

Complete rotor stop — mu = 0 (Lanzon 2015 catastrophic case).

MotorEfficiencyDecay(trigger_time, rotor_id, label=None, tau=1.0, mu_floor=0.0) dataclass

Bases: DamageEvent

Exponential effectiveness decay starting from trigger_time.

From the trigger onward, mu[i] evolves as

.. math:: \dot \mu_i = -(1/\tau)(\mu_i - \mu_\text{floor}),

so mu exponentially approaches mu_floor with time-constant tau. Applies a one-shot setter at trigger time; the actual decay is integrated by :meth:RotorDamageState.step_decay on each env step.

RotorDamageManager(profile=None)

Owns :class:RotorDamageState and replays events over the episode.

Used by the env layer: on every integrator tick the env calls :meth:update with the current and previous timestamps; events that fall in that window are applied to the state. Time-decay rotors are advanced one Euler step.

reset(*, seed=None)

Clear all damage and re-baseline (called by env.reset).

seed is accepted for compatibility with gym.Env.reset.

inject_event(event)

Add a one-shot event for this episode (single-fire).

update(t_current, t_previous, dt)

Apply events in (t_previous, t_current] and advance decay.

Parameters:

Name Type Description Default
t_current float

time at the END of the integrator step.

 required
t_previous float

time at the START of the integrator step.

 required
dt float

integrator step size (used by state.step_decay).

 required

Returns:

Type Description
list[DamageEvent]

List of events that fired during this step (for logging).

NonlinearQuadrotorEnv(initial_state, number_time_steps, *, dt=0.01, integrator='rk4', action_space='virtual', allocator=None, damage_profile=None, damage_event_callback=None, omega_min=0.0, omega_max=1000.0)

Bases: Env

Gymnasium env over the pure-numpy nonlinear 6-DoF quadrotor.

Parameters:

Name Type Description Default
initial_state ndarray

12-element initial state. See :mod:tensoraerospace.aerospacemodel.quadrotor.nonlinear for the layout.

 required
number_time_steps int

Episode length cap (steps).

 required
dt float

Discretisation step (s). Default 0.01.

 0.01
integrator Literal['euler', 'rk4']

"euler" or "rk4" (default).

 'rk4'
action_space Literal['virtual', 'rotor']

"virtual" (4 = T+τ) or "rotor" (4 = ω² per motor). See module docstring.

 'virtual'
allocator Optional[XConfigAllocator]

X-config allocator to use when action_space="rotor" or when damage is active. None → :func:default_allocator.

 None
damage_profile Optional[DamageProfile]

Optional :class:DamageProfile to apply over the episode.

 None
damage_event_callback Optional[Callable[[Any, Any], None]]

Optional f(event, state) called after each event triggers (for logging / TensorBoard).

 None
omega_min float

Lower rotor-speed bound, rad/s. Used in the env's saturation step before mixing. Default 0.

 0.0
omega_max float

Upper rotor-speed bound, rad/s. Default 1000.

 1000.0