Attitude Control — the inner loop

1 · The 5-minute recap

A quadrotor is underactuated: 4 motors give you total thrust \(T\) and three body torques \( \boldsymbol\tau=(\tau_x,\tau_y,\tau_z) \), but you have 6 DOF to manage. You can't push sideways — you tilt, then thrust. So attitude is the innermost, fastest loop; everything else rides on it.

To drive the attitude \( \boldsymbol\eta=(\phi,\theta,\psi) \) to a desired \( \boldsymbol\eta_d \), use a PD law (the rate \( \boldsymbol\omega \) is the damping term):

\[ \boldsymbol\tau = \mathbf{I}\Big( K_p^{\text{att}}(\boldsymbol\eta_d-\boldsymbol\eta) - K_d^{\text{att}}\,\boldsymbol\omega \Big) \]

Per axis this is a second-order system: \( \omega_n=\sqrt{K_p},\ \zeta=K_d/(2\sqrt{K_p}) \). Aim for \( \zeta\approx 1 \) — fast with no overshoot. Build the intuition here first:

2 · Setup (2 min)

From the simulator/ directory, in your virtual environment:

# one-time
python -m venv .venv && source .venv/bin/activate   # Windows: .venv\Scripts\activate
pip install -r requirements.txt
# every session — so `import quadsim` works
export PYTHONPATH=.                                # Windows: set PYTHONPATH=.

The file you edit all term is quadsim/controllers/student.py. Open it now — you'll see a StudentController with a hover-thrust stub and two TODOs. We fill the Week-5 one.

3 · Build the inner loop

Work through the steps. Each adds a few lines to StudentController.control(self, t, x, ref). Recall the state layout: x[0:3] position, x[3:6] velocity, x[6:9] Euler \((\phi,\theta,\psi)\), x[9:12] body rates \((p,q,r)\).

euler = x[6:9]            # (phi, theta, psi)  [rad]
omega = x[9:12]           # body rates (p, q, r) [rad/s]

yaw_ref  = float(ref.get("yaw", 0.0))
att_des  = np.array([0.0, 0.0, yaw_ref])   # level, hold yaw

e_att = att_des - euler
e_att[2] = (e_att[2] + np.pi) % (2*np.pi) - np.pi      # wrap yaw error
tau = self.p.inertia * (self.kp_att * e_att + self.kd_att * (-omega))

T = self.p.weight                       # = mass * g
return np.array([T, tau[0], tau[1], tau[2]])

self.kp_att = np.array([180.0, 180.0, 80.0])
self.kd_att = np.array([ 28.0,  28.0, 20.0])

def control(self, t, x, ref):
    p = self.p
    euler = x[6:9]
    omega = x[9:12]

    yaw_ref = float(ref.get("yaw", 0.0))
    att_des = np.array([0.0, 0.0, yaw_ref])        # hold level

    e_att = att_des - euler
    e_att[2] = (e_att[2] + np.pi) % (2*np.pi) - np.pi
    tau = p.inertia * (self.kp_att * e_att + self.kd_att * (-omega))

    T = p.weight
    return np.array([T, tau[0], tau[1], tau[2]])

4 · Run & verify

Save this as examples/wk5_attitude.py. It starts the quad tilted and watches it level out — the exact checkpoint for this lab:

# examples/wk5_attitude.py — Week 5 checkpoint: return to level from a tilt
import numpy as np
from quadsim import Simulator, QuadParams
from quadsim.dynamics import hover_state
from quadsim.controllers import StudentController

sim = Simulator(QuadParams())
x0 = hover_state(position=(0, 0, 1.0))
x0[6] = np.deg2rad(20.0)     # start rolled  +20 deg
x0[7] = np.deg2rad(-15.0)    # start pitched -15 deg

log = sim.run(x0=x0, controller=StudentController(sim.params),
              reference=lambda t: dict(pos=[0, 0, 1.0], yaw=0.0), t_final=4.0)

roll  = np.rad2deg(log.euler[:, 0])
pitch = np.rad2deg(log.euler[:, 1])
print(f"start  roll/pitch = {roll[0]:6.1f} / {pitch[0]:6.1f} deg")
print(f"final  roll/pitch = {roll[-1]:6.3f} / {pitch[-1]:6.3f} deg")

from quadsim.plotting import plot_states
plot_states(log, save="wk5_attitude.png", show=False)
print("saved wk5_attitude.png")

export PYTHONPATH=.
python examples/wk5_attitude.py

Also run the shared hover example to see the bigger picture — it levels and stays airborne, but drifts in position (there's no outer loop yet — that's Week 6's job, and the motivation for it):

python examples/02_hover_pid.py --controller student

Self-check

• From a 20° tilt the final attitude is within 0.1° of level.
• With kd_att set to zero, I saw it oscillate (proving D is the damping).
• I can explain why we damp with -omega instead of differentiating the angle error.
• I understand the quad still drifts in position — and why that needs next week's outer loop.

5 · Going further (optional)

• Damping sweep. Re-run with \(K_d \in \{8, 16, 28, 50\}\). Plot settling time vs \(K_d\); find where overshoot disappears (\(\zeta\!\approx\!1\)).
• Yaw step. Change yaw in the reference to 30° and confirm the wrap term keeps the turn short.
• Break it. Set \(K_p=600\). What happens, and which actuator limit is it hitting? (Look at log.f — the per-motor thrusts saturating at f_max.)

6 · Deliverable & submission

Next week → Week 6: the outer position loop. You keep editing the same StudentController — by then it holds a setpoint, and by the final project it flies a mission.