Trajectory Tracking — fly a figure-eight

1 · The 5-minute recap

A reference trajectory is not just a point — it carries position and its derivatives: \( \mathbf p_r(t),\ \mathbf v_r(t),\ \mathbf a_r(t) \). The figure-eight is the classic multirotor benchmark (world is z-up / ENU, so height is \(+z\)):

\[ \mathbf p_r(t)=\begin{bmatrix} a\sin\omega t \\ b\sin 2\omega t \\ h \end{bmatrix},\qquad \omega=\frac{2\pi}{T},\qquad \mathbf v_r=\dot{\mathbf p}_r,\quad \mathbf a_r=\ddot{\mathbf p}_r. \]

Chasing a moving target with feedback alone always lags. The cure is feedforward: feed the reference acceleration straight into the desired world acceleration, so feedback only corrects what's left:

\[ \mathbf a_{\text{des}} \;=\; \mathbf a_r \;+\; K_p\,(\mathbf p_r-\mathbf p)\;+\;K_d\,(\mathbf v_r-\mathbf v). \]

That is exactly the line already in quadsim/controllers/cascade_pid.py: a_des = acc_ref + self.kp_pos * e_pos + self.kd_pos * e_vel. Thrust then solves the vertical accel, \( T=m\,(g+a_{\text{des},z}) \) — gravity is \(-z\), thrust is \(+\)body-\(z\).

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

This week you mostly run the reference autopilot (CascadePID) on a trajectory rather than editing it — the goal is to see feedforward and wind in action and to read the tracking numbers. The ready-made example is examples/03_figure8.py.

3 · Track the figure-eight

Work through the steps. The reference generators live in quadsim/trajectories.py; each returns a function ref(t) → dict(pos, vel, acc, yaw) you can hand straight to sim.run(...).

from quadsim import trajectories as traj
ref = traj.figure_eight(a=1.0, b=0.5, height=1.5, period=10.0)
# ref(t) -> {"pos": [...], "vel": [...], "acc": [...], "yaw": 0.0}

from quadsim import Simulator, QuadParams
from quadsim.controllers import CascadePID
from quadsim.dynamics import hover_state

sim = Simulator(QuadParams())
log = sim.run(x0=hover_state(position=(0, 0, 1.5)),
              controller=CascadePID(sim.params),
              reference=ref, t_final=20.0)
print(f"no wind  : RMSE = {log.position_rmse():.3f} m")   # ~0.037 m

import numpy as np
wind = lambda t: np.array([2.0, 0.0, 0.0])     # 2 N along world +x
log_w = sim.run(x0=hover_state(position=(0, 0, 1.5)),
                controller=CascadePID(sim.params),
                reference=ref, t_final=20.0, wind=wind)
print(f"with wind: RMSE = {log_w.position_rmse():.3f} m")

from quadsim.plotting import plot_pose_strobe, plot_tracking_error
plot_pose_strobe(log,   save="wk9_strobe.png",      show=False)
plot_tracking_error(log, save="wk9_tracking.png",   show=False)

export PYTHONPATH=.
python examples/03_figure8.py --plot                 # no wind  → ~0.037 m
python examples/03_figure8.py --plot --wind 2.0      # with wind → ~0.48 m

4 · Run & verify

Save this as examples/wk9_trajectory.py. It flies the figure-eight twice — clean air and a wind gust — prints both RMSEs, and saves the two deliverable figures:

# examples/wk9_trajectory.py — Week 9 checkpoint: track a figure-eight, no-wind vs wind
import numpy as np
from quadsim import Simulator, QuadParams
from quadsim.controllers import CascadePID
from quadsim.dynamics import hover_state
from quadsim import trajectories as traj
from quadsim.plotting import plot_pose_strobe, plot_tracking_error

sim = Simulator(QuadParams())
ref = traj.figure_eight(a=1.0, b=0.5, height=1.5, period=10.0)

# 1) no wind
log = sim.run(x0=hover_state(position=(0, 0, 1.5)),
              controller=CascadePID(sim.params), reference=ref, t_final=20.0)
print(f"no wind  : RMSE = {log.position_rmse():.3f} m")

# 2) constant 2 N world-x wind
wind = lambda t: np.array([2.0, 0.0, 0.0])
log_w = sim.run(x0=hover_state(position=(0, 0, 1.5)),
                controller=CascadePID(sim.params), reference=ref, t_final=20.0, wind=wind)
print(f"with wind: RMSE = {log_w.position_rmse():.3f} m")

# 3) deliverable figures (from the no-wind run)
plot_pose_strobe(log,    save="wk9_strobe.png",   show=False)
plot_tracking_error(log, save="wk9_tracking.png", show=False)
print("saved wk9_strobe.png and wk9_tracking.png")

export PYTHONPATH=.
python examples/wk9_trajectory.py

Self-check

• My no-wind figure-eight RMSE is ≈ 0.037 m (a few cm).
• Turning on wind made the RMSE rise, and I can say why (feedforward can't anticipate the gust).
• I can point to the feedforward term acc_ref in a_des and explain why it beats pure feedback on a moving target.
• My strobe and tracking-error PNGs both rendered, with the flown path near the dashed reference.
• I can sketch the Wk 1–9 map (frames → 6-DOF → trim/linearize → cascade → LQR → estimation → tracking) for the midterm.

5 · Going further (optional)

• Kill the feedforward. Temporarily ignore acc/vel in the outer loop (pure P/D) and re-measure — watch the RMSE jump. This is the whole argument for feedforward.
• Speed it up. Halve the period (faster eight). Tracking error grows with target speed — by how much?
• Other references. Swap in traj.waypoints([...]) or traj.step(...) and compare how the same autopilot copes with a non-smooth target.
• Midterm warm-up. For each arrow in the Wk 1–9 map, write the one convention hazard that lives there (NED↔ENU sign, plus↔X mixer, quaternion order, gimbal lock at \( \theta=\pm90° \)).

6 · Deliverable & submission

Next week → Week 10: the intelligent-control half begins. The ~0.037 m figure-eight RMSE you just measured is the baseline that MPC and learning-based controllers will try to beat. Reference: Beard & McLain UAVbook (mavsim_public) and simulator/CONVENTIONS.md.