M5-T5: Attitude control

Learning objectives

After this lesson, you should be able to:

• Describe the cascaded angle-rate attitude controller and its tuning order.
• Define the attitude error on SO(3) and write the geometric control law.
• Explain the roles of inertia, torque limits and motor lag in attainable bandwidth.
• Simulate large-angle recovery with the geometric controller.

Conceptual explanation

The cascaded controller (as in PX4 and ArduPilot) has an outer angle loop ($P$ controller turning attitude error into a desired body rate) and an inner rate loop (PID turning rate error into torque), lesson M5-T6. The geometric controller (Lee, Leok, McClamroch 2010) defines the error directly on the rotation group and computes the torque in one step, with almost-global stability on SO(3).

Engineering intuition

The attitude plant is a double integrator (torque to angular acceleration to rate to angle) with limits: the motors can deliver bounded torque, so there is a maximum angular acceleration $a_{\max}=\tau _{\max}/J$. A proportional-derivative law sets a natural frequency $\omega _{n}=\sqrt{K_{p}}$ and damping; asking for $\omega _{n}$ beyond what the torque limit and motor lag allow saturates the motors and makes the loop slow and oscillatory (lesson M1-T6). For large errors a saturated, rate-limited angle loop (a square-root controller) turns the vehicle as fast as it can without overshoot.

Mathematical formulation

Geometric attitude control for desired attitude $R_{d}$ and angular velocity $\Omega _{d}$, current $R$ and $\Omega$ (body frame):

For small errors $e_{R}$ ≈ sin of the rotation angle times the axis, so the law is a PD controller with natural frequency $\sqrt{k_{R}/J}$ and damping $k_{\Omega }/(2\sqrt{k_{R} J})$. The error function has an unstable equilibrium at a $180^{\circ}$ error (the “antipodal” attitude), so convergence is almost global: a small perturbation leaves it. Cascade equivalent: angle loop $\Omega _{\text{cmd}}=K_{p}\,e$ (saturated at $\Omega _{\max}$) and rate loop $\tau =J(K_{d}(\Omega _{\text{cmd}}-\Omega )+\ldots )$.

$e_{R}$, $e_{\Omega }$

attitude and rate errors.

$k_{R}$, $k_{\Omega }$

positive gains.

$J$

inertia matrix.

∨

vee map (skew matrix to vector).

Architecture and data flow

Position controller → desired thrust direction (and yaw) → $R_{d}$ and thrust magnitude → attitude controller → torque → mixer (lesson M1-T6). PX4 implements the cascaded form (multicopter attitude and rate controllers, with parameters in the PID tuning guide); the geometric form appears in research controllers and in trajectory-tracking papers that fly aggressive manoeuvres.

Real-world UAV applications

• Large-angle recovery after a disturbance or flip.
• Aggressive trajectory tracking where small-angle linearisation fails.
• Analysing attainable bandwidth from the torque limit.
• Replacing Euler-angle controllers that have singularities.

Implementation example

This runs as shown: the geometric controller recovers from a $120^{\circ}$ attitude error to level in about a second with bounded torque, and the closed-loop energy decreases monotonically.

import numpy as np
hat = lambda w: np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])
vee = lambda S: np.array([S[2, 1], S[0, 2], S[1, 0]])
def Exp(w):
    th = np.linalg.norm(w)
    if th < 1e-12: return np.eye(3) + hat(w)
    K = hat(w / th); return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * K @ K

J = np.diag([0.029, 0.029, 0.055]); kR, kW = 30.0 * 0.029, 8.5 * 0.029       # gains scaled by inertia: natural frequency about sqrt(30) rad/s, damping near 0.78
tau_max = 0.5; dt = 0.002
R = Exp(np.array([1.2, -0.8, 0.9]) / np.linalg.norm([1.2, -0.8, 0.9]) * np.radians(120)); W = np.zeros(3); Rd = np.eye(3)
hist = []; torque_peak = 0.0
for k in range(int(4.0 / dt)):
    eR = 0.5 * vee(Rd.T @ R - R.T @ Rd); eW = W                           # desired rate is zero
    M = -kR * eR - kW * eW + np.cross(W, J @ W)
    M = np.clip(M, -tau_max, tau_max); torque_peak = max(torque_peak, np.abs(M).max())
    W = W + np.linalg.solve(J, M - np.cross(W, J @ W)) * dt; R = R @ Exp(W * dt)
    ang = np.degrees(np.arccos(np.clip((np.trace(Rd.T @ R) - 1) / 2, -1, 1))); hist.append(ang)
hist = np.array(hist); t_settle = np.argmax(hist[::-1] > 2.0)
print(f"initial error {hist[0]:.0f} deg, below 2 deg after {(len(hist) - t_settle) * dt:.2f} s, final {hist[-1]:.4f} deg, peak torque {torque_peak:.3f} N m")
assert hist[0] > 100 and hist[-1] < 0.1 and (len(hist) - t_settle) * dt < 3.0
assert np.allclose(R @ R.T, np.eye(3), atol=1e-9)                          # the rotation stays on SO(3) throughout

Common engineering mistakes

Failure modes

Debugging approach

1. Step the attitude setpoint and log angle, rate and torque; check the torque saturation flags.
2. Estimate $a_{\max}=\tau _{\max}/J$ and compare with the requested acceleration.
3. Test large errors $(90^{\circ},\,150^{\circ})$ in simulation.
4. Compare cascade and geometric on the same manoeuvre.

Practical exercises

1. Derive the small-angle PD form and the natural frequency in terms of $k_{R}$ and $J$.
2. Find the largest initial error from which the example controller still converges and explain the $180^{\circ}$ case.
3. Add a first-order motor lag to the example and re-tune.
4. Compute $a_{\max}$ for $\tau _{\max}=0.5\,N\cdot m$ and $J_{x}=0.029\,\mathrm{kg}\cdot m^{2}$.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• T. Lee, M. Leok, N. H. McClamroch, “Geometric tracking control of a quadrotor UAV on SE(3)”, CDC 2010
• PX4: multicopter PID tuning guide
• R. Mahony, V. Kumar, P. Corke, IEEE RAM 2012

What’s next