M5-T14: Trajectory tracking

Learning objectives

After this lesson, you should be able to:

• Write the position tracking law with feedback and acceleration feedforward and the closed-loop error dynamics.
• Construct the desired thrust vector and the desired attitude from it, including the yaw reference.
• Compute standard tracking error metrics.
• Explain why the tracking error depends on the reference’s derivatives and the inner-loop bandwidth.

Conceptual explanation

Established The thrust-vector construction follows the geometric tracking control literature (Lee, Leok and McClamroch 2010) and common autopilot practice. Specific autopilots implement variants with different saturation and priority rules.

Given a reference $p_{r}(t)$ with derivatives $v_{r}$ and $a_{r}$, define the errors $e_{p}=p-p_{r}$ and $e_{v}=v-v_{r}$. A PD law with acceleration feedforward commands

With the inner loops fast enough to treat the commanded acceleration as achieved, the error obeys $\ddot{e}_{p}+K_{v}\,\dot{e}_{p}+K_{p}\,e_{p}=0$, a linear, exponentially stable system with no dependence on the trajectory. Without the feedforward term the error obeys the same equation driven by −$a_{r}$, so the tracking error is proportional to the reference acceleration.

Engineering intuition

Chasing a car by only looking at the gap always leaves you behind: you accelerate when the gap has already opened. Knowing the car’s planned acceleration lets you accelerate at the same moment. That is feedforward. With it the feedback only has to correct disturbances and model errors, so the gains can be modest and the response smooth.

The outer loop cannot be faster than the inner. If the attitude loop takes 0.15 s to realise a commanded tilt, the position loop assumes a wrong acceleration for that long. Tracking aggressive references demands bandwidth in the inner loops, or a reference whose derivatives stay within what they deliver.

Mathematical formulation

For a multirotor with mass $m$, total thrust $T$ along the body $z$ axis and the world frame with $z$ up, $m\,\ddot{p}=T\,R\,e_{3}-m\,g\,e_{3}$. The desired thrust vector from the commanded acceleration is

Given a desired yaw $\psi _{r}$, the other axes follow from a heading vector $c=(\cos \,\psi _{r},\,\sin \,\psi _{r},\,0)$:

The construction is singular when $b_{3,\,\text{des}}$ is parallel to $c$, which is only the case when the desired thrust is horizontal (more than $90^{\circ}$ of tilt, outside any normal envelope). Practical autopilots limit the tilt and prioritise the thrust direction over yaw when the limit is reached. Common metrics for evaluating tracking are the RMS and maximum position error:

$K_{p}$, $K_{v}$

position and velocity gains $(1/s^{2},\,1/s)$.

$b_{3}$,des

desired body $z$ axis (thrust direction).

$\psi _{r}$

reference yaw angle.

Architecture and data flow

Trajectory generator (or planner) → sample $p_{r}$, $v_{r}$, $a_{r}$, $\psi _{r}$ at the controller’s rate → position controller (feedback plus feedforward) → thrust magnitude and desired attitude → attitude controller → rate controller → motor mixer. The estimator supplies $p$ and $v$; its latency and noise enter the error directly, so tracking performance cannot be better than estimation quality allows. In PX4 offboard mode the trajectory setpoint message carries position, velocity and acceleration fields, so feedforward terms can be supplied (check the documentation of your version for how they are combined).

Real-world UAV applications

• Following minimum-snap trajectories in aggressive flight.
• Inspection paths with a required position accuracy.
• Formation and coordinated flight following reference time laws.
• Testing controllers by comparing error metrics on standard figure-eight references.

Implementation example

This runs as shown. A point-mass model with a first-order lag (0.1 s) between commanded and achieved acceleration tracks a circle of radius 5 m at 3 m/s. The test shows that feedforward of the reference acceleration reduces the RMS error, and checks the thrust-vector construction gives an orthonormal $R_{\text{des}}$ with the right thrust direction.

import numpy as np

def ref(t, r=5.0, w=0.6):
    return (np.array([r * np.cos(w * t), r * np.sin(w * t)]), np.array([-r * w * np.sin(w * t), r * w * np.cos(w * t)]),
            np.array([-r * w * w * np.cos(w * t), -r * w * w * np.sin(w * t)]))
def run(ff, Kp=9.0, Kv=6.0, tau=0.1, dt=0.005, T=20.0):
    p, v, a = np.array([5.0, 0.0]), np.array([0.0, 3.0]), np.array([-1.8, 0.0]); errs = []
    for k in range(int(T / dt)):
        pr, vr, ar = ref(k * dt)
        a_cmd = (ar if ff else 0.0) - Kp * (p - pr) - Kv * (v - vr)
        a += (a_cmd - a) / tau * dt; v = v + a * dt; p = p + v * dt            # first-order lag between commanded and achieved acceleration
        if k * dt > 5: errs.append(np.linalg.norm(p - pr))
    return np.sqrt(np.mean(np.square(errs))), max(errs)
(r0, m0), (r1, m1) = run(False), run(True)
print(f"RMS error without feedforward {r0:.3f} m, with feedforward {r1:.3f} m (max {m1:.3f} m)")
assert r1 < 0.5 * r0
def desired_attitude(a_cmd, yaw, m=1.5, g=9.81):
    F = m * (a_cmd + np.array([0, 0, g])); b3 = F / np.linalg.norm(F)
    c = np.array([np.cos(yaw), np.sin(yaw), 0.0]); b2 = np.cross(b3, c); b2 /= np.linalg.norm(b2); b1 = np.cross(b2, b3)
    return np.column_stack([b1, b2, b3]), np.linalg.norm(F)
R, T = desired_attitude(np.array([2.0, -1.0, 0.5]), 0.7)
print(f"thrust {T:.2f} N, orthonormal: {np.allclose(R.T @ R, np.eye(3), atol=1e-12)}, det {np.linalg.det(R):.3f}")
assert np.allclose(R.T @ R, np.eye(3), atol=1e-12) and abs(np.linalg.det(R) - 1) < 1e-12 and np.allclose(R[:, 2] * T, 1.5 * np.array([2.0, -1.0, 10.31]))

Common engineering mistakes

Failure modes

Debugging approach

1. Plot position error against time and against reference acceleration: correlation indicates missing feedforward.
2. Check the clocks: shift the reference by $\Delta t$ and see whether the error decreases.
3. Plot commanded against achieved tilt to measure inner-loop lag.
4. Check thrust and tilt saturation flags in the log.

Practical exercises

1. Sweep the circular reference angular rate from 0.3 to 1.2 rad/s and plot the RMS error with and without feedforward.
2. Add a 50 ms estimation delay and show the effect on the RMS error.
3. Increase the lag to 0.3 s and tune the gains to restore stability.
4. Implement tilt saturation at $30^{\circ}$ and report the change in tracking error at 1.2 rad/s.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• Lee, T., Leok, M. & McClamroch, N. H. (2010). Geometric tracking control of a quadrotor UAV on SE(3). IEEE CDC
• Mellinger, D. & Kumar, V. (2011). Minimum snap trajectory generation and control for quadrotors. ICRA
• PX4: Offboard mode (multicopter)

What’s next