Week 6 Lab — Position & Altitude Control

Goal Last week your controller returned the quad to level but it still drifted in position. Today you add the outer position/altitude loop on top of that inner loop. By the end it holds a hover setpoint and then tracks a step — with a final position error under 0.1 m, matching the reference CascadePID autopilot.

1 · The 5-minute recap

A quadrotor is underactuated: you can't push sideways, you tilt then thrust. Last week's inner loop drove attitude \( \boldsymbol\eta=(\phi,\theta,\psi) \) to a setpoint — but we set \( \phi_d=\theta_d=0 \) by hand, so the quad only ever hovered in place and drifted. The outer loop now computes that setpoint from position error.

Treat the centre of mass as a point you steer with acceleration. A PD on world-frame position (feed-forward + integral on z) gives the desired acceleration:

\[ \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} \]

The world is z-up / ENU (gravity \(-z\), thrust \(+\)body-\(z\)), so the vertical channel sets a gravity-compensated thrust, and the horizontal channels set the desired tilt (rotated by yaw, clipped):

\[ T = m\,(g + a_{des,z}), \qquad \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) \]

Two sign traps Our world is z-up (ENU), so \( T=m(g+a_{des,z}) \) — not the NED form from Beard & McLain (z-down), where the sign flips. And the vehicle is X-frame, not plus-frame: the simulator's mixer handles that for you, so never paste a plus-frame mixer.

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=.

You keep editing the same quadsim/controllers/student.py from Week 5. Your inner attitude loop should already be there — today we add the outer loop above it and feed it the computed \( \phi_{des},\theta_{des},T \).

3 · Build the outer loop

Each step adds a few lines to StudentController.control(self, t, x, ref) (and a couple to __init__/reset). 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)\).

1 Add outer-loop gains & an integrator edit __init__ / reset

Next to your Week-5 attitude gains, add position gains, an altitude integral gain, and a tilt limit. The integral state is reset between runs:

# in __init__ (Week 6)
self.kp_pos  = np.array([6.0, 6.0, 12.0])
self.kd_pos  = np.array([4.0, 4.0,  8.0])
self.ki_z    = 2.0
self.max_tilt = np.deg2rad(30.0)        # safety clamp on commanded tilt

# in reset()
self._iz = 0.0
self._t_prev = None

2 Read the references and split the state edit control()

Pull position & velocity from the state, and the setpoints from ref (with safe defaults). Compute the timestep dt for the integrator:

pos, vel   = x[0:3], x[3:6]
euler, omega = x[6:9], x[9:12]

pos_ref = np.asarray(ref.get("pos", [0.0, 0.0, 0.0]), float)
yaw_ref = float(ref.get("yaw", 0.0))
vel_ref = np.asarray(ref.get("vel", [0.0, 0.0, 0.0]), float)
acc_ref = np.asarray(ref.get("acc", [0.0, 0.0, 0.0]), float)

dt = 0.0 if self._t_prev is None else t - self._t_prev
self._t_prev = t

3 Position error → desired acceleration the outer PD

Feed-forward + PD on position, plus a clamped integral on the altitude channel only (it trims steady bias so the quad holds height exactly):

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

4 Vertical channel → gravity-compensated thrust z-up!

In z-up coordinates the thrust must cancel gravity and add the vertical demand. Floor it so the quad never commands (near-)zero thrust and free-falls:

T = self.p.mass * (self.p.g + a_des[2])   # gravity comp: a_des[2]=0 -> T = m*g
T = max(T, 0.1 * self.p.weight)

5 Horizontal channel → desired roll/pitch small-angle + yaw

The small-angle map turns horizontal acceleration into tilt, rotated by the commanded yaw, then clipped to the tilt limit. The result is the setpoint for your Week-5 inner loop:

ax, ay = a_des[0], a_des[1]
phi_des   = (1.0/self.p.g) * (ax*np.sin(yaw_ref) - ay*np.cos(yaw_ref))
theta_des = (1.0/self.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])

6 Feed the inner loop & return the wrench reuse Week 5

Your Week-5 inner loop is unchanged — but its setpoint is now the computed att_des instead of hard-coded level. Wrap the yaw error, run the attitude PD, return \( [T,\tau_x,\tau_y,\tau_z] \):

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))
return np.array([T, tau[0], tau[1], tau[2]])

✓ Stuck? Reveal the full Week-6 control()

Try first You learn the cascade by writing it. Peek only after a genuine attempt.

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

    pos_ref = np.asarray(ref.get("pos", [0.0, 0.0, 0.0]), float)
    yaw_ref = float(ref.get("yaw", 0.0))
    vel_ref = np.asarray(ref.get("vel", [0.0, 0.0, 0.0]), float)
    acc_ref = np.asarray(ref.get("acc", [0.0, 0.0, 0.0]), float)

    dt = 0.0 if self._t_prev is None else t - self._t_prev
    self._t_prev = t

    # --- OUTER loop: position error -> desired acceleration ---
    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 loop (Week 5): attitude PD -> body torques ---
    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))

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

4 · Run & verify

The shared example holds a hover then commands a step to [1, 1, 1.5] — the exact checkpoint for this lab. Run your controller, then the reference, and compare:

export PYTHONPATH=.
python examples/02_hover_pid.py --controller student     # your cascade
python examples/02_hover_pid.py                           # reference CascadePID baseline

Each run prints the position RMSE and the final position. For a side-by-side plot of your run, add --plot (it saves hover_student.png):

python examples/02_hover_pid.py --controller student --plot

Expected — your graded checkpoint ⭐

Your controller holds the hover, then climbs/translates to [1, 1, 1.5] and settles there.
Final position error < 0.1 m vs the target — on par with the reference CascadePID (no steady offset, thanks to the altitude integral).
The saved hover_student.png shows x, y, z converging smoothly with no sustained oscillation and no overshoot past the tilt limit.

Self-check

The final position is within 0.1 m of [1, 1, 1.5], matching the CascadePID baseline.
With the altitude integral ki_z set to zero, I saw a small steady height offset — proving why I needs it.
I can explain why thrust is \( T=m(g+a_{des,z}) \) and how the z-up convention sets that sign.
I understand that \( \phi_{des},\theta_{des} \) computed here are exactly the setpoint my Week-5 inner loop already tracks.

5 · Going further (optional)

Tilt-limit probe. Command a far setpoint (e.g. [5, 0, 1.5]). Watch \( \phi_{des},\theta_{des} \) hit the 30° clip and the climb slow — that's the saturation keeping the small-angle map valid.
Integral off. Set ki_z = 0 and confirm a small steady altitude error appears; restore it and watch the offset vanish.
Gain sweep. Halve and double kp_pos; plot settling time vs gain and find where overshoot creeps in (the outer loop must stay slower than the inner one).

6 · Deliverable & submission

Criterion	What we look for	Weight
Correct outer loop	Position PD → \( \mathbf{a}_{des} \); gravity-comp thrust; small-angle tilt map with yaw rotation & clip	50%
Result & tuning	Holds hover then step with final error < 0.1 m; the checkpoint plot, no sustained oscillation	25%
Understanding	One short paragraph: why \( T=m(g+a_{des,z}) \) and how horizontal accel maps to desired tilt	25%

📤 Submit via the MUST LMS

⭐ This submission is graded (5%). Full requirements & rubric: Lab 3 assignment brief.

What

student.py + hover_student.png + a 3–5 line note (one PDF or zip)

Filename

MLTE03_Wk6_<studentID>.zip

Where

MUST LMS → MLTE03 → Week 6 Lab dropbox LMS link — TO FILL

Deadline

date / time — TO FILL (before Week 7)

Submissions are handled entirely in the MUST LMS — nothing is uploaded to this site.

References: Quan Quan RFly multicopter decks · rfly.buaa.edu.cn/course.html · PX4 controller diagrams (production cascade autopilot) · docs.px4.io/…/controller_diagrams.

Next week → Week 7. You keep editing the same StudentController — it now flies a setpoint; soon it tracks a trajectory, and the intelligent-control labs improve on this exact baseline.