MLTE03 · Week 6 · Position & Altitude Control

MLTE03 · Week 6 · Lecture + Lab

Position & Altitude Control

The full cascade autopilot · 位置與高度控制：完整串級自駕儀

Flight Dynamics & Intelligent Control Technologies
Wrap an outer loop around last week's attitude loop — now it holds a point.

Recap → today

Last week we stopped it tipping. It still drifts.

  • Wk 5: the inner attitude loop — PD on \( (\phi,\theta,\psi) \) → body torques \( \boldsymbol\tau \). It returns to level.
  • But with level commanded, the quad just hovers wherever it happens to be — and drifts with any push or bias.
  • Today: the outer position/altitude loop computes which way to tilt and how hard to thrust.
The missing piece:
Wk 5 set \( \phi_d=\theta_d=0 \) by hand. Today the outer loop computes \( \phi_d,\theta_d \) and \(T\) from position error — that's the whole autopilot.

Learning objectives

By the end of today you can…

  • Build the outer loop: position error → desired world acceleration \( \mathbf{a}_{des} \).
  • Get thrust from the vertical channel with gravity compensation: \( T=m(g+a_{des,z}) \).
  • Map horizontal \( \mathbf{a}_{des} \) → desired roll/pitch via the small-angle relation, rotated by yaw and clipped to a tilt limit.
  • Wrap this outer loop around the Week-5 inner loop → the complete cascade autopilot.
  • Lab Add the outer loop to StudentController; hold a hover, then track a step.

Architecture

The cascade, completed

   pos ref ─►┌───────────────┐  φ_d,θ_d  ┌───────────────┐ τx,τy,τz ┌──────────┐
             │  OUTER loop   ├──+ thrust─►│  INNER loop   ├─────────►│  mixer   ├─► motors
  (slow~50Hz)│ pos→a_des→tilt│     T      │  attitude PD  │ (fast)   └──────────┘
             └▲──────────────┘            └▲──────────────┘
     pos,vel ─┘  state x          attitude ─┘  (Week 5)
  • This week: the OUTER loop — turn position error into \( \phi_d,\theta_d \) and \(T\). The inner loop is exactly your Week-5 code.
  • Separation of timescales: the outer loop is slower, so it treats the fast inner loop as "instant". Same structure as the PX4 cascade autopilot.

Step 1 of the outer loop

Position error → desired acceleration

Treat the quad's centre of mass as a point you steer with acceleration. A PD law on world-frame position (with feed-forward, and an integral on the vertical channel to kill steady bias):

\[ \mathbf{a}_{des} = \mathbf{a}_{ff} + K_p^{pos}\,\mathbf{e}_{pos} + K_d^{pos}\,\mathbf{e}_{vel}, \qquad \mathbf{e}_{pos}=\mathbf{p}_{ref}-\mathbf{p},\;\; \mathbf{e}_{vel}=\mathbf{v}_{ref}-\mathbf{v} \] \[ a_{des,z} \mathrel{+}= K_i^{z}\!\int e_{pos,z}\,dt \quad\text{(integral on altitude only)} \]
World axes are z-up / ENU (gravity is \(-z\), thrust is \(+\)body-\(z\)). Same line you'll write:
a_des = acc_ref + kp_pos*e_pos + kd_pos*e_vel; a_des[2] += ki_z*iz

Step 2 · the vertical channel

Altitude → gravity-compensated thrust

Newton in world-z (z-up): \( m\,\ddot z = T_{world,z} - mg \). To realise \( a_{des,z} \) near hover (where thrust points roughly up), set the total thrust to cancel gravity and add the demand:

\[ \boxed{\,T = m\,(g + a_{des,z})\,} \]
\( a_{des,z}=0 \Rightarrow T=mg \) — exact hover thrust (the weight). The integral term quietly trims any modelling error so it holds altitude.
Floor it for safety: \( T=\max(T,\,0.1\,mg) \) so the quad never commands negative/zero thrust and free-falls.

T = p.mass * (p.g + a_des[2]); T = max(T, 0.1*p.weight)

Step 3 · the horizontal channels

Horizontal accel → desired roll/pitch

To accelerate sideways you must tilt: the tilted thrust vector has a horizontal component. Near hover (small angles, \(T\approx mg\)) the world-horizontal accelerations map to tilt, rotated by the yaw \( \psi \):

\[ \phi_{des} = \tfrac{1}{g}\big(a_x\sin\psi - a_y\cos\psi\big), \qquad \theta_{des} = \tfrac{1}{g}\big(a_x\cos\psi + a_y\sin\psi\big) \] \[ \phi_{des},\theta_{des} \leftarrow \mathrm{clip}\big(\cdot,\ -\theta_{max},\ +\theta_{max}\big), \qquad \theta_{max}=30^\circ \]
\( (\phi_{des},\theta_{des},\psi_{ref}) \) becomes the setpoint of your Week-5 inner loop. The clip is a safety limit — the small-angle map breaks down (and motors saturate) past ~30°.

Tied to the simulator

The reference: cascade_pid.py

Exactly the structure you'll implement — outer loop on top, your Week-5 inner loop underneath:


# OUTER: desired world acceleration = feed-forward + PD (+ I on z)
e_pos = pos_ref - pos
e_vel = vel_ref - vel
self._iz = np.clip(self._iz + e_pos[2]*dt, -2.0, 2.0)
a_des = acc_ref + self.kp_pos*e_pos + self.kd_pos*e_vel
a_des[2] += self.ki_z * self._iz

T = p.mass * (p.g + a_des[2])           # gravity-compensated thrust
T = max(T, 0.1 * p.weight)

ax, ay = a_des[0], a_des[1]             # horizontal accel -> tilt, rotated by yaw
phi_des   = (1.0/p.g) * (ax*np.sin(yaw_ref) - ay*np.cos(yaw_ref))
theta_des = (1.0/p.g) * (ax*np.cos(yaw_ref) + ay*np.sin(yaw_ref))
phi_des   = np.clip(phi_des,   -self.max_tilt, self.max_tilt)
theta_des = np.clip(theta_des, -self.max_tilt, self.max_tilt)
att_des = np.array([phi_des, theta_des, yaw_ref])

# INNER (Week 5): attitude PD -> body torques
e_att = att_des - euler
e_att[2] = (e_att[2] + np.pi) % (2*np.pi) - np.pi   # wrap yaw error
tau = p.inertia * (self.kp_att*e_att + self.kd_att*(-omega))
return np.array([T, tau[0], tau[1], tau[2]])

Don't get bitten

Two sign traps in this loop

z-up, not NED. Our world is ENU / z-up: gravity is \(-z\), thrust is \(+\)body-\(z\), so \(T=m(g+a_{des,z})\). Beard & McLain use NED (z-down) — lift an equation from there and the z-sign is wrong.
X-frame mixer, not plus. Many references give the plus-frame mixer (roll = motors 2&4 only). Our quad is X-frame — every motor contributes to roll and pitch. The simulator handles it; just never paste a plus-frame mixer.

Euler is intrinsic ZYX (yaw→pitch→roll); attitude singularity at pitch \(=\pm90°\). Stay near hover and you never see it.

Second half · hands-on

Now you build the outer loop

  • Open the Week 6 lab sheet → keep editing quadsim/controllers/student.py.
  • Add the outer loop on top of your Week-5 inner loop: \( \mathbf{a}_{des} \to T,\ \phi_{des},\theta_{des} \).
  • Run python examples/02_hover_pid.py --controller student.
  • Checkpoint: hold a hover, then a step — final position error < 0.1 m vs the CascadePID baseline. ⭐ graded.

Open the Week 6 lab sheet →

Wrap-up

What to remember

  • Outer loop = a PD on position → desired acceleration: \( \mathbf{a}_{des}=\mathbf{a}_{ff}+K_p^{pos}\mathbf{e}_{pos}+K_d^{pos}\mathbf{e}_{vel} \) (+ I on z).
  • Vertical channel → gravity-compensated thrust: \( T=m(g+a_{des,z}) \), floored for safety.
  • Horizontal channel → desired tilt via the small-angle map (rotated by yaw, clipped to \( \theta_{max} \)).
  • Feed \( (\phi_{des},\theta_{des},\psi_{ref}) \) into the Week-5 inner loop → the complete cascade autopilot.
  • Next: trajectory tracking, then the intelligent-control labs that improve on this baseline.

References: Quan Quan RFly decks · rfly.buaa.edu.cn/course.html · PX4 controller diagrams · docs.px4.io/…/controller_diagrams. Deliverable & deadline on the lab sheet.