MLTE03 · Week 9 · Lecture + Lab

Trajectory Tracking & Consolidation

Feedforward, a figure-eight, & midterm review · 軌跡跟蹤與整合；期中複習

Flight Dynamics & Intelligent Control Technologies
Block: Know where you are — stitch Weeks 1–9 into one autopilot that flies a curve.

Recap → today

From holding a point to flying a curve

Wk 5–6: the cascade — inner attitude loop, outer position loop — holds a fixed setpoint.
Wk 7–8: optimal control (LQR) & estimation (the EKF feeding clean state to the loop).
Today: the setpoint moves. Track a smooth reference \( \big(\mathbf p_r(t),\mathbf v_r(t),\mathbf a_r(t)\big) \).

The catch: feedback only reacts after error appears. Chasing a moving target with P/D alone always lags. The fix is feedforward — anticipate, don't just react.

Learning objectives

By the end of today you can…

Build a smooth reference trajectory that carries position, velocity and acceleration.
Explain why feedforward beats pure feedback on a moving target — and add it.
Track a figure-eight with vel/acc feedforward; add wind and read the tracking RMSE.
Render the 3-D flight strobe and the tracking-error plot from the simulator.
Review Consolidate the flight-dynamics half (Wks 1–9) for the midterm.

The reference

A trajectory carries pos, vel & acc

quadsim.trajectories ships hover, step, waypoints, figure_eight.
Each returns ref(t) → dict(pos, vel, acc, yaw) — the derivatives are the point.
The figure-eight is the classic multirotor tracking benchmark:

\[ \mathbf p_r(t)=\begin{bmatrix} a\sin\omega t \\ b\sin 2\omega t \\ h \end{bmatrix},\quad \omega=\tfrac{2\pi}{T} \] Differentiate analytically for \( \mathbf v_r=\dot{\mathbf p}_r \) and \( \mathbf a_r=\ddot{\mathbf p}_r \) — no numerical noise.

Defaults: \( a=1,\ b=0.5,\ h=1.5\ \text{m},\ T=10\ \text{s} \). World is z-up / ENU — height is \(+z\).

The idea

Feedforward: anticipate, don't just react

The outer loop already computes a desired world acceleration. Add the reference acceleration as a feedforward term so the loop leads the target instead of trailing it:

\[ \mathbf a_{\text{des}} \;=\; \underbrace{\mathbf a_r}_{\text{feedforward}} \;+\; \underbrace{K_p\,(\mathbf p_r-\mathbf p) \;+\; K_d\,(\mathbf v_r-\mathbf v)}_{\text{feedback corrects what's left}} \]

Pure feedback ⇒ a standing position error that grows with target speed (lag).

With \( \mathbf a_r \) fed forward, feedback only mops up model error & wind — far smaller.

This is exactly the a_des = acc_ref + kp_pos·e_pos + kd_pos·e_vel line in cascade_pid.py.

How accel becomes a command

Desired accel → thrust + tilt

The same outer-loop map you built in Week 6, now driven by the feedforward \( \mathbf a_{\text{des}} \):

\[ T = m\,(g + a_{\text{des},z}), \qquad \phi_d \approx \tfrac{1}{g}\big(a_x\sin\psi - a_y\cos\psi\big),\quad \theta_d \approx \tfrac{1}{g}\big(a_x\cos\psi + a_y\sin\psi\big) \]

Convention hazards (CONVENTIONS.md): world is z-up (gravity \(-z\), thrust \(+\)body-\(z\)); intrinsic ZYX Euler; the mixer is X-frame — every motor contributes to roll and pitch. Do not paste a plus-frame mixer (Bresciani / Gibiansky) here.

Worked · the simulator

Fly the figure-eight in `quadsim`

from quadsim import Simulator, QuadParams
from quadsim.controllers import CascadePID
from quadsim.dynamics import hover_state
from quadsim import trajectories as traj

sim = Simulator(QuadParams())
ref = traj.figure_eight(a=1.0, b=0.5, height=1.5, period=10.0)
log = sim.run(x0=hover_state(position=(0, 0, 1.5)),
              controller=CascadePID(sim.params),
              reference=ref, t_final=20.0)
print(f"tracking RMSE = {log.position_rmse():.3f} m")   # ~0.037 m, no wind

Equivalently from the shell: python examples/03_figure8.py --plot (add --wind 2.0 for a gust).

See the flight

Strobe & tracking error

plot_pose_strobe(log) — a long-exposure 3-D PNG: the quad's X-frame drawn at intervals, colour = time, reference dashed.
plot_tracking_error(log) — per-axis error \(e_x,e_y,e_z\), the norm \(\lVert e\rVert\), and the RMSE line.
Read the numbers: ~0.037 m no wind; it rises sharply (≈ 0.48 m at 2 N wind).

   z
   ▲     ____         ____
   │    /    \  ✈    /    \
   │   |      \____ /      |   ← figure-eight, h = 1.5 m
   │    \      /    \     /
   │     ‾‾‾‾        ‾‾‾‾
   └──────────────────────► x,y
   colour = time · dashed = reference

Consolidation

The flight-dynamics half, in one map

  Wk1 frames/quats ─► Wk2-3 6-DOF model ẋ=f(x,u) ─► Wk4 trim & linearize (A,B)
        │                                                      │
        ▼                                                      ▼
  Wk5 attitude (PD) ─► Wk6 position (cascade) ─► Wk7 LQR ─► Wk8 estimation (EKF)
                                  │
                                  ▼
                       Wk9 trajectory tracking + feedforward  ◄── you are here

Frames/conventions: z-up ENU, ZYX Euler, scalar-first quaternions, X-frame mixer, gimbal lock at \( \theta=\pm90° \).
Modelling → control → estimation: the unstable open loop, the cascade that tames it, optimal gains, clean state.

Second half · hands-on

Now you fly the curve

Track trajectories.figure_eight() with vel/acc feedforward.
Run python examples/03_figure8.py --plot, then add --wind 2.0.
Render plot_pose_strobe + plot_tracking_error.
Report: tracking RMSE no-wind vs wind (no-wind ≈ 0.037 m; 2 N wind ≈ 0.48 m). ⭐ graded.

Open the Week 9 lab sheet →

Wrap-up

What to remember

A reference trajectory carries pos, vel & acc — the derivatives enable feedforward.
Feedforward beats pure feedback on a moving target: \( \mathbf a_{\text{des}}=\mathbf a_r + K_p\mathbf e_p + K_d\mathbf e_v \).
The reference autopilot tracks the figure-eight to ~0.037 m RMSE (no wind); wind raises it (≈ 0.48 m at 2 N).
You can now assemble Weeks 1–9 into one accumulated autopilot — the midterm baseline.
Next: the intelligent-control half — MPC & learning-based controllers that aim to beat this RMSE.

Refs: Beard & McLain UAVbook (mavsim_public, Ch. 6 — but z-down NED, flip the z-sign) · simulator/CONVENTIONS.md. Deliverable & deadline on the lab sheet.