Quadcopter Drone Control Tutorial

Introduction

This tutorial starts from mathematical fundamentals and gradually builds a complete control system for quadrotor drones. The core problems addressed in each chapter and their interrelationships are as follows:

Reading Recommendations:

• Quick Start (understanding basic control): 1 → 2 → 4 → 5
• Comprehensive Learning: Follow the order 1 → 8
• Special Topic Reading: 3 (differential flatness) is prerequisite knowledge for understanding the trajectory planning tutorial

Control tutorial (8 code blocks):

1. Coordinate system and rotation basics — 3D rotation visualization: UAV “+” shape rotation around Z/Y/X axes

2. Dynamic modeling — Given the speeds of 4 motors, calculate thrust/moment, and visualize the thrust direction.

III. Differential flatness — spiral trajectory → Derive attitude angle and thrust from flat output

4. PID Control — Comparison of the Impact of Kp/Kd/Ki on the Step Response of Second-Order Systems

5. Model-based Control — Simulation of 2D UAV Feedforward + PD Control in Y-Z Plane

6. Three-dimensional trajectory tracking — 3D U-shaped trajectory tracking, expected vs actual comparison

7. SE(3) Control — Large Angle Flip: Comparison of Euler Angle Control and SE(3) Control Error

8. MPC Control — 1D Altitude Control MPC, showing the prediction interval and constraint handling

1. Coordinate System and Rotation Basics

Before studying the quadcopter drone, we need to understand coordinate system transformation and rotation matrix first. These are the mathematical foundations for describing the drone’s attitude.

1.1 Geometric Derivation of Two-Dimensional Coordinate Rotation

Consider a point on a plane $A(x, y)$. When the coordinate system rotates by an angle $\theta$, the coordinates of this point in the new coordinate system are $A'(x', y')$.

Method 1: Geometric Projection Method

Let the original coordinate system be $OXY$, and the new coordinate system be $OX'Y'$ (rotated counterclockwise by $\theta$). Through the geometric projection relationship:

Write in matrix form:

Method 2: Polar Coordinate Method

The point $A$ is $r$ away from the origin, and its angle with the $X$ axis is $\alpha$. After rotation:

Write in matrix form:

Personal understanding: Method 2 involves rotating around the origin counterclockwise by $\theta$ degrees. Therefore, the rotation matrix is the transpose of Method 1 (axis rotation).

1.2 Differences between the two rotation modes

Mode 1: Axis rotation (point remains fixed, coordinate system rotates)

When the original coordinate system remains unchanged and the axes rotate, find the coordinates of the point in the new coordinate system:

Mode 2: Point Rotation (Coordinate system unchanged, point rotated)

Personal understanding: From a different perspective—finding the coordinates of a point in a new coordinate system is equivalent to rotating the coordinate axes clockwise by $\theta$. You can imagine standing on the coordinate axis and rotating with it; the positions of the points around you will naturally change.

Personal understanding: The rotation matrices for both modes are transposes of each other. Mode 1 can be understood as “active coordinate system rotation”, while Mode 2 is “active point rotation”. In drones, we usually use Mode 1—we know how many degrees the body has rotated, and we want to find the representation of the same point in the new coordinate system.

1.3 3D Rotation Matrix

Promote 2D to 3D, and rotate around three axes respectively:

Rotate around the $Z$ axis (Yaw, yaw angle $\psi$):

Rotate around the $Y$ axis (Pitch, pitch angle $\theta$):

Rotate around the $X$ axis (Roll, roll angle $\phi$):

1.4 UAV Coordinate System Transformation

Personal understanding: In the drone scenario, it is a case of “coordinate axis rotation” — the body coordinate system rotates relative to the world coordinate system. We need to convert the vectors in the body frame into expressions in the world frame.

Then, the meaning of rotating from the own coordinate axis is $R_c = R_o^T \cdot R_r$.

If $R_o$ is known for $R_c$, and $R_c$ is the inverse coordinate axis, then $R_o$ can be derived. However, this must be for the rotation of the world coordinate.

Personal understanding: In drone applications, the rotation matrix $R$ usually represents the transformation from the body frame to the world frame, that is $\mathbf{p}_W = R \cdot \mathbf{p}_B$. Its inverse transformation (world→body) is $R^T$, because the rotation matrix is an orthogonal matrix, $R^{-1} = R^T$.

Python Validation

pythonimport numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题

# ========== 可视化无人机"+"十字形绕 Z/Y/X 轴旋转 ==========

# --- 构造无人机"+"形状（4 个臂端 + 中心） ---
L = 1.0  # 臂长
drone = np.array([
    [ L, 0, 0],  # 右臂端
    [-L, 0, 0],  # 左臂端
    [ 0, L, 0],  # 前臂端
    [ 0,-L, 0],  # 后臂端
    [ 0, 0, 0],  # 中心
]).T  # 3×5 矩阵，每列一个点

# --- 三维旋转矩阵 ---
def Rz(psi):
    """绕 Z 轴旋转（偏航角 ψ）"""
    c, s = np.cos(psi), np.sin(psi)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

def Ry(theta):
    """绕 Y 轴旋转（俯仰角 θ）"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])

def Rx(phi):
    """绕 X 轴旋转（滚转角 φ）"""
    c, s = np.cos(phi), np.sin(phi)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

angle = np.radians(45)  # 旋转 45°

# --- 绘制 4 个子图：原始 + 3 种旋转 ---
titles = ['原始姿态', f'绕 Z 轴旋转 {int(np.degrees(angle))}°（偏航）',
          f'绕 Y 轴旋转 {int(np.degrees(angle))}°（俯仰）',
          f'绕 X 轴旋转 {int(np.degrees(angle))}°（滚转）']
rotations = [np.eye(3), Rz(angle), Ry(angle), Rx(angle)]

fig = plt.figure(figsize=(14, 10))
for i, (R, title) in enumerate(zip(rotations, titles)):
    ax = fig.add_subplot(2, 2, i + 1, projection='3d')
    pts = R @ drone  # 旋转后的点坐标

    # 画两条臂（十字形）
    ax.plot([pts[0,0], pts[0,1]], [pts[1,0], pts[1,1]],
            [pts[2,0], pts[2,1]], 'b-o', lw=3, label='臂 1')
    ax.plot([pts[0,2], pts[0,3]], [pts[1,2], pts[1,3]],
            [pts[2,2], pts[2,3]], 'r-o', lw=3, label='臂 2')
    # 标注中心
    ax.scatter(*pts[:, 4], color='k', s=80, zorder=5)

    # 画旋转后的机体坐标轴
    axis_len = 0.6
    for j, (color, lbl) in enumerate(zip(['r','g','b'], ['Xb','Yb','Zb'])):
        axis_vec = R @ (axis_len * np.eye(3)[:, j])
        ax.quiver(0, 0, 0, *axis_vec, color=color, arrow_length_ratio=0.15)

    ax.set_xlim(-1.5, 1.5); ax.set_ylim(-1.5, 1.5); ax.set_zlim(-1.5, 1.5)
    ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')
    ax.set_title(title)

plt.suptitle('无人机"+"十字形的三维旋转演示', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control1.png', dpi=150, bbox_inches='tight')
plt.show()

Execution result:

II. Dynamics Modeling of the Four-rotor Vehicle

2.1 Motor Model

The four-rotor aircraft is powered by four motors, and its model is as follows:

Motor speed model (first-order inertia link):

Personal understanding: The motor speed model is provided by the manufacturer, and closed loop control is already implemented internally. We don’t need to worry about its internal implementation during modeling; we can use it directly. The coefficient $k_m$ can be calibrated through actual measurements.

Thrust generated by a single motor:

Counter-torque generated by a single motor:

Here, $k_m$ is the motor response constant, $k_F$ is the thrust coefficient, and $k_M$ is the moment coefficient.

Personal understanding: Thrust and torque are proportional to the square of the speed, which is a fundamental conclusion in aerodynamics. In actual systems, $k_F$ and $k_M$ are calibrated through bench tests. The motor model has a first-order inertial element; the larger $k_m$, the faster the motor response. In controller design, it is usually assumed that the motor response is fast enough to ignore this dynamics.

2.2 Z-X-Y Euler angles and rotation matrix

Use the Z-X-Y order of Euler angles to represent rotation (first yaw $\psi$, then roll $\phi$, and finally pitch $\theta$):

Expanded (resulting in $c = \cos$, $s = \sin$):

2.3 Relationship between Angular Velocity and Euler Angle Derivatives

The relationship between the body angular velocity $[\omega_x, \omega_y, \omega_z]^T$ and the Euler angle derivative $[\dot{\phi}, \dot{\theta}, \dot{\psi}]^T$ is:

Personal understanding: This transformation matrix exhibits a singularity (Gimbal Lock) at $\phi = \pm 90°$, which is an inherent flaw in the Euler angle representation. For small angular movements (such as near hovering), the matrix approximates to an identity matrix, meaning the angular velocity is approximately equal to the derivative of the Euler angles.

2.4 Newton’s Equation (Translational Dynamics)

Personal understanding: The Newton-Euler equation is formulated in an inertial coordinate system and describes the motion of a drone under the action of resultant force and resultant torque.

Newton’s equation describes the translation of the center of mass:

Personal understanding: The $m\ddot{\mathbf{p}}$ on the left side of the equal sign represents the center of mass acceleration multiplied by mass; the first item on the right is gravity (vertically downward), and the second item is the rotation matrix $R$ that converts the total thrust along the axis direction of the body $z$ into the world coordinate system (note that the thrust direction always lies along the positive direction of the $z$ axis). Air resistance is ignored here.

Where $u_1 = F_1 + F_2 + F_3 + F_4$ represents the total thrust. By expanding each component, we can obtain:

2.5 Euler’s Equation (Rotational Dynamics)

The right side is defined as control moment inputs:

Personal understanding: The Euler equations are established in the body frame, because the moment of inertia matrix $I$ is a constant diagonal matrix in the body frame, which avoids the complexity due to posture changes.

Personal understanding: The $\omega \times I\omega$ item is a Coriolis torque/gyro effect term. It can be ignored near hover (when angular velocity is low), providing a basis for subsequent linearization. $u_2, u_3$ generates roll/pitch moments through differential thrust, while $u_4$ generates yaw moments through differential counter-torque.

2.6 Summary of the Dynamics Model

2.7 Example of the Plane State Equation

Taking the motion in the plane (Y-Z plane) as an example, the state vector $\mathbf{x} = [y, z, \phi, \dot{y}, \dot{z}, \dot{\phi}]^T$, input $\mathbf{u} = [u_1, u_2]^T$:

Personal understanding: This is the nonlinear state equation in two-dimensional conditions. It can be seen that the acceleration in the $y$ direction depends on $\sin\phi$ (it needs to be tilted first to move horizontally), the acceleration in the $z$ direction depends on $\cos\phi$ (the vertical component of the thrust), and $\phi$ is directly controlled by the torque $u_2$. This reflects the “underdriven” nature of the quadcopter—only 4 inputs are used to control 6 degrees of freedom.

Python Validation

pythonimport numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题

# ========== 四旋翼电机模型：推力/力矩计算与可视化 ==========

# --- 电机参数 ---
kF = 6.11e-8   # 推力系数 (N/(rad/s)^2)
kM = 1.5e-9    # 力矩系数 (N·m/(rad/s)^2)
L  = 0.175     # 臂长 (m)

# --- 四个电机转速 (rad/s)，模拟非对称飞行 ---
omega = np.array([4000, 4200, 3800, 4100])  # ω1, ω2, ω3, ω4

# --- 计算各电机推力和反扭矩 ---
F = kF * omega**2     # 各电机推力
M = kM * omega**2     # 各电机反扭矩

# --- 计算总推力和控制力矩 ---
u1 = np.sum(F)                       # 总推力
u2 = L * (F[1] - F[3])              # 滚转力矩
u3 = L * (F[2] - F[0])              # 俯仰力矩
u4 = M[0] - M[1] + M[2] - M[3]     # 偏航力矩

print(f"各电机推力: F = {F} N")
print(f"总推力 u1 = {u1:.4f} N")
print(f"滚转力矩 u2 = {u2:.6f} N·m")
print(f"俯仰力矩 u3 = {u3:.6f} N·m")
print(f"偏航力矩 u4 = {u4:.8f} N·m")

# --- 可视化：俯视图（电机位置 + 推力）和 3D 推力箭头 ---
fig = plt.figure(figsize=(14, 5))

# 子图 1：俯视图，四个电机位置和推力大小
ax1 = fig.add_subplot(1, 2, 1)
motor_pos = np.array([[0, L], [-L, 0], [0, -L], [L, 0]])  # 电机 1前 2左 3后 4右
colors = ['#e74c3c', '#3498db', '#2ecc71', '#f39c12']
for i in range(4):
    circle = plt.Circle(motor_pos[i], 0.04, color=colors[i], zorder=5)
    ax1.add_patch(circle)
    ax1.annotate(f'M{i+1}\nω={omega[i]}\nF={F[i]:.3f}N',
                 motor_pos[i], textcoords="offset points",
                 xytext=(15, 10), fontsize=8, color=colors[i])
# 画机体十字
ax1.plot([motor_pos[1,0], motor_pos[3,0]],
         [motor_pos[1,1], motor_pos[3,1]], 'k-', lw=2)
ax1.plot([motor_pos[0,0], motor_pos[2,0]],
         [motor_pos[0,1], motor_pos[2,1]], 'k-', lw=2)
ax1.set_xlim(-0.35, 0.35); ax1.set_ylim(-0.35, 0.35)
ax1.set_aspect('equal'); ax1.grid(True, alpha=0.3)
ax1.set_title('俯视图：电机分布与推力'); ax1.set_xlabel('Y (m)'); ax1.set_ylabel('X (m)')

# 子图 2：3D 视图，显示推力箭头
ax2 = fig.add_subplot(1, 2, 2, projection='3d')
for i in range(4):
    x, y = motor_pos[i]
    # 各电机推力箭头（沿 Z 轴向上）
    scale = F[i] / np.max(F) * 0.3  # 归一化显示
    ax2.quiver(x, y, 0, 0, 0, scale, color=colors[i],
               arrow_length_ratio=0.2, lw=2, label=f'M{i+1}: {F[i]:.3f}N')
    ax2.scatter(x, y, 0, color=colors[i], s=60)
# 总推力箭头（从中心出发）
ax2.quiver(0, 0, 0, 0, 0, 0.4, color='k', arrow_length_ratio=0.15,
           lw=3, label=f'总推力 u1={u1:.3f}N')
ax2.set_xlabel('Y'); ax2.set_ylabel('X'); ax2.set_zlabel('Z（推力方向）')
ax2.set_title('3D 视图：推力矢量'); ax2.legend(fontsize=7, loc='upper left')

plt.suptitle('四旋翼电机模型可视化', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control2.png', dpi=150, bbox_inches='tight')
plt.show()

Execution result:

III. Differential Flatness

3.1 Definition and Significance of Differential Flatness

Personal understanding: Differential flatness serves as a bridge between trajectory planning and control—with differential flatness, planning only requires designing a flat output trajectory, and all states and inputs can be derived automatically. Combined with SE(3) control, very precise pose tracking can be achieved.

Differential Flatness refers to the following: If all states and inputs of a system can be represented by a set of Flat Outputs and their finite derivatives, then the system is differential flat.

Personal understanding: The four-rotor has a 12-dimensional trajectory and 4-dimensional inputs, but the flat output is only 4-dimensional ($x, y, z, \psi$). The significance of differential flatness is that if all the system’s states and inputs can be expressed by a set of “flat outputs” and their finite derivatives, then the high-dimensional control problem can be simplified to a trajectory design problem for these 4 flat outputs.

3.2 Selection of Flat Output

For a four-rotor drone, select flat output as:

It defines a sufficiently wide set of trajectories:

Personal understanding: The flat output $\sigma$ defines a trajectory on $SE(3)$—that is, a complete rigid body motion trajectory that includes both position ($\mathbb{R}^3$) and orientation ($SO(3)$). The transformation from the world coordinate system to the body coordinate system can be fully determined by $\sigma$ and its derivatives.

The complete state vector is:

Personal understanding: By selecting $\sigma = [x, y, z]^T$ and $\dot{\sigma} = [\dot{x}, \dot{y}, \dot{z}]^T$, it is possible to parse out roll, pitch, yaw, and angular velocity.

3.3 Derive all states from flat output

The core value of differential flatness is that, given $\sigma(t) = [x(t), y(t), z(t), \psi(t)]$ and its derivatives, all 12 states and 4 inputs of the system can be derived. The derivation process is as follows:

Personal understanding: This is why minimizing snap (the fourth derivative) is crucial in trajectory planning—snap is directly related to the magnitude of the control torque. If snap is too large, it means a large control torque is required, and the motor may become saturated.

3.4 Derivation of Pose Angles

Starting from Newton’s equation:

Derive the total thrust $u_1$

Personal understanding: The total thrust $u_1$ is equal to the modulus of the resultant force: $u_1 = m\sqrt{\ddot{x}^2 + \ddot{y}^2 + (\ddot{z}+g)^2}$. Physically, it means that the total thrust must overcome gravity simultaneously and provide the required acceleration.

Thus, the axis direction of the body $z$ is obtained:

Here, $\mathbf{t}$ represents the unit thrust direction vector.

Build the desired rotation matrix

In the direction determined by the yaw angle $\psi$, through the intermediate variable $\hat{x}_C$:

Personal understanding: The approach to constructing the rotation matrix is as follows—first determine the thrust direction $\hat{z}_B$, and then determine the auxiliary direction based on the yaw angle $\hat{x}_C$. $\hat{y}_B$ is obtained through the cross product $\hat{z}_B \times \hat{x}_C$, and it is perpendicular to the plane formed by $\hat{z}_B$ and $\hat{x}_C$. Finally, $\hat{x}_B = \hat{y}_B \times \hat{z}_B$ completes the right-handed coordinate system.

Extract Euler angles

Extract each Euler angle from the rotation matrix:

Personal understanding: Therefore, $\phi, \theta$ is entirely determined by the second derivative (acceleration) of the flat output $[x, y, z, \psi]$, without the need for higher derivatives.

3.5 Derivation of Angular Velocities p, q, r

Personal understanding: Next, to derive the angular velocity $p, q, r$, we need to take another time derivative of Newton’s equation (from acceleration to jerk).

Take the time derivative (jerk) of Newton’s equation:

Personal understanding: $\hat{z}_W$ is a fixed unit vector in the world coordinate system, which does not change over time, so $\frac{d}{dt}(-mg\hat{z}_W) = 0$.

Since $\hat{z}_W$ is a constant vector, its derivative is zero, therefore:

Personal understanding: When differentiating $u_1\hat{z}_B$, use the product rule: $\dot{u}_1$ is the rate of change of thrust magnitude, and $\dot{\hat{z}}_B$ is the rate of change of thrust direction. According to rigid body kinematics, the rate of change of the direction vector can be expressed as the cross product of angular velocity: $\dot{\hat{z}}_B = \omega_{BW} \times \hat{z}_B$.

Using kinematic relationships $\dot{\hat{z}}_B = \omega_{BW} \times \hat{z}_B$:

Remove the component in the direction of $\hat{z}_B$(https://example.com/ee_keep_0000): (subtract by taking the difference after multiplying both sides by $\hat{z}_B$):

Define auxiliary vector:

Personal understanding: $h_W$ is a auxiliary vector calculated from the known jerk and thrust. It contains the component information of $\omega_{BW}$ in the plane perpendicular to $\hat{z}_B$, from which $p$ and $q$ can be extracted.

Extract the components p, q, and r

Angular velocity decomposed under the machine system:

Personal understanding: Note that $\omega_{BW} = p\hat{x}_B + q\hat{y}_B + r\hat{z}_B$ is vector decomposition. Utilizing the orthogonality of $\hat{x}_B, \hat{y}_B, \hat{z}_B$, each component can be extracted by dot product.

We know $\hat{x}_B \cdot \hat{y}_B = \|\hat{x}_B\|\|\hat{y}_B\|\cos 90° = 0$, while $\hat{x}_B$ and $\hat{y}_B$ are orthogonal coordinates, $\cos\alpha = 0$.

Dot product with the unit vector of each axis using $h_W = \omega_{BW} \times \hat{z}_B / u_1$:

For the $r$ component—$\Gamma$ is the component of angular velocity $\omega_{BW}$ in the $\hat{z}_B$ direction:

Personal understanding: Due to orthogonality $\hat{x}_B \perp \hat{z}_B$, $\hat{y}_B \perp \hat{z}_B$, the angular velocity component $r$ in the $\hat{z}_B$ direction cannot be obtained from $h_W$ (a quantity perpendicular to $\hat{z}_B$), and must be calculated separately using the yaw rate $\dot{\psi}$.

Personal understanding: The $r$ component is directly related to the yaw rate $\dot{\psi}$. This indicates that the yaw angle is one of the independent flat outputs. $p, q$ requires jerk (third-order derivative) to be calculated, while $r$ only needs $\dot{\psi}$—this also reflects the decoupling of the yaw axis.

Python Validation

pythonimport numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== 微分平坦性验证：从圆形轨迹推导姿态角和推力 ==========

# --- 物理参数 ---
m = 0.5    # 无人机质量 (kg)
g = 9.81   # 重力加速度 (m/s^2)

# --- 定义圆形轨迹（平坦输出 σ = [x, y, z, ψ]） ---
T_total = 10.0              # 总时间 (s)
dt = 0.01
t = np.arange(0, T_total, dt)
omega_c = 2 * np.pi / T_total  # 圆轨迹角频率

# 位置轨迹：水平圆 + 缓慢上升 + 偏航跟随速度方向
radius = 2.0
x = radius * np.cos(omega_c * t)
y = radius * np.sin(omega_c * t)
z = 0.5 * t                                # 螺旋上升
psi = np.arctan2(np.gradient(y, dt), np.gradient(x, dt))  # 偏航角跟随速度方向

# --- 计算各阶导数 ---
ddx = np.gradient(np.gradient(x, dt), dt)  # x 的二阶导数（加速度）
ddy = np.gradient(np.gradient(y, dt), dt)  # y 的二阶导数
ddz = np.gradient(np.gradient(z, dt), dt)  # z 的二阶导数

# --- 由微分平坦性推导总推力 u1 ---
u1 = m * np.sqrt(ddx**2 + ddy**2 + (ddz + g)**2)

# --- 由加速度推导姿态角 φ (roll) 和 θ (pitch) ---
# 机体 z 轴方向（单位推力方向）
tx = ddx / (ddz + g + 1e-10)
ty = ddy / (ddz + g + 1e-10)

# 通过旋转矩阵关系推导欧拉角
phi   = np.arcsin(np.clip(tx * np.sin(psi) - ty * np.cos(psi), -1, 1))  # 滚转角
theta = np.arctan2(tx * np.cos(psi) + ty * np.sin(psi),
                   1 + ddz / g)  # 俯仰角

# --- 绘制结果 ---
fig = plt.figure(figsize=(15, 10))

# 子图 1：三维轨迹
ax1 = fig.add_subplot(2, 2, 1, projection='3d')
ax1.plot(x, y, z, 'b-', lw=1.5)
# 每隔一段标注无人机位置和朝向
step = len(t) // 8
for i in range(0, len(t), step):
    ax1.scatter(x[i], y[i], z[i], c='r', s=40, zorder=5)
    dx_arrow = 0.3 * np.cos(psi[i])
    dy_arrow = 0.3 * np.sin(psi[i])
    ax1.quiver(x[i], y[i], z[i], dx_arrow, dy_arrow, 0,
               color='r', arrow_length_ratio=0.3)
ax1.set_xlabel('X (m)'); ax1.set_ylabel('Y (m)'); ax1.set_zlabel('Z (m)')
ax1.set_title('螺旋上升轨迹 σ(t)')

# 子图 2：总推力 u1
ax2 = fig.add_subplot(2, 2, 2)
ax2.plot(t, u1, 'g-', lw=1.5)
ax2.axhline(y=m*g, color='k', ls='--', alpha=0.5, label=f'悬停推力 mg={m*g:.2f}N')
ax2.set_xlabel('时间 (s)'); ax2.set_ylabel('u₁ (N)')
ax2.set_title('总推力 u₁（由二阶导数推出）'); ax2.legend(); ax2.grid(True, alpha=0.3)

# 子图 3：滚转角 φ
ax3 = fig.add_subplot(2, 2, 3)
ax3.plot(t, np.degrees(phi), 'r-', lw=1.5, label='φ (roll)')
ax3.set_xlabel('时间 (s)'); ax3.set_ylabel('角度 (°)')
ax3.set_title('滚转角 φ（由 σ̈ 推出）'); ax3.legend(); ax3.grid(True, alpha=0.3)

# 子图 4：俯仰角 θ
ax4 = fig.add_subplot(2, 2, 4)
ax4.plot(t, np.degrees(theta), 'b-', lw=1.5, label='θ (pitch)')
ax4.set_xlabel('时间 (s)'); ax4.set_ylabel('角度 (°)')
ax4.set_title('俯仰角 θ（由 σ̈ 推出）'); ax4.legend(); ax4.grid(True, alpha=0.3)

plt.suptitle('微分平坦性验证：从轨迹 [x,y,z,ψ] 推导姿态与推力', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control3.png', dpi=150, bbox_inches='tight')
plt.show()

Execution result:

IV. Basics of PID Control

4.1 The Impact of PID Parameters on System Response

The three parameters of the PID controller $K_p$, $K_d$, and $K_i$ affect different response characteristics of the system:

| Parameter | Rise Time | Overshoot | Settling Time | Steady-state Error | |------|----------------|----------------|--------------------------| | $K_p \uparrow$ | Reduce | Increase | Reduce | Reduce | | $K_d \uparrow$ | — | Reduce | Reduce | — | | $K_i \uparrow$ | Increase | Increase | Increase | — |

4.2 Step Response Characteristics

Key indicators of step response:

• Rise Time (Rise time): The time it takes for the response to rise from the initial value to the steady-state value
• Overshoot (Overshoot): The maximum deviation where the response exceeds the steady-state value
• Settling Time (Settling time): The time it takes for the response to enter and remain within the range of the steady-state value $\pm 1\%$ (or $\pm 5\%$)
• Steady-state Error (Steady-state error): The difference between the response value and the expected value at steady state

Personal understanding: There are two common standards for Settling Time: the time required to enter the $\pm 5\%$ range or the $\pm 1\%$ range. In engineering, the $\pm 2\%$ standard is more commonly used.

Personal understanding: $K_p$ reduces the rise time but increases overshoot; $K_d$ increases damping and reduces overshoot; $K_i$ eliminates steady-state error but may cause oscillations. In quadcopter control, it is crucial to properly adjust these three parameters—usually start by adjusting $K_p$ to ensure response speed, then adjust $K_d$ to suppress overshoot, and finally add an appropriate amount of $K_i$ to eliminate steady-state error.

Python Validation

pythonimport numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== PID 参数调节对二阶系统阶跃响应的影响 ==========
# 被控对象：无人机高度方向简化模型  m * z̈ = u - mg
# 等效二阶系统：ë + (Kd/m)*ė + (Kp/m)*e = -(Ki/m)*∫e dt

def simulate_pid(Kp, Kd, Ki, dt=0.001, T=5.0):
    """仿真 PID 控制的二阶系统阶跃响应"""
    steps = int(T / dt)
    z = 0.0       # 当前位置（高度）
    dz = 0.0      # 当前速度
    z_des = 1.0   # 期望高度（阶跃输入）
    m = 0.5       # 无人机质量
    g = 9.81      # 重力加速度
    integral = 0.0  # 积分项

    z_hist = np.zeros(steps)
    for i in range(steps):
        e = z_des - z           # 位置误差
        de = -dz                # 速度误差（期望速度为 0）
        integral += e * dt      # 误差积分

        # PID 控制律：u = mg + Kp*e + Kd*ė + Ki*∫e
        u = m * g + Kp * e + Kd * de + Ki * integral

        # 动力学：m * z̈ = u - mg
        ddz = (u - m * g) / m
        dz += ddz * dt
        z += dz * dt
        z_hist[i] = z
    return z_hist

dt = 0.001; T = 5.0
t = np.arange(0, T, dt)

# --- 子图 1：改变 Kp（固定 Kd=2, Ki=0） ---
fig, axes = plt.subplots(1, 3, figsize=(16, 4.5))

ax = axes[0]
for Kp in [1, 5, 15, 30]:
    z_hist = simulate_pid(Kp=Kp, Kd=2.0, Ki=0.0, dt=dt, T=T)
    ax.plot(t, z_hist, label=f'Kp={Kp}')
ax.axhline(y=1.0, color='k', ls='--', alpha=0.4, label='目标')
ax.set_title('改变 Kp（Kd=2, Ki=0）')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('高度 z (m)')
ax.legend(fontsize=8); ax.grid(True, alpha=0.3)
ax.set_ylim(-0.1, 1.8)

# --- 子图 2：改变 Kd（固定 Kp=15, Ki=0） ---
ax = axes[1]
for Kd in [0, 1, 3, 8]:
    z_hist = simulate_pid(Kp=15.0, Kd=Kd, Ki=0.0, dt=dt, T=T)
    ax.plot(t, z_hist, label=f'Kd={Kd}')
ax.axhline(y=1.0, color='k', ls='--', alpha=0.4, label='目标')
ax.set_title('改变 Kd（Kp=15, Ki=0）')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('高度 z (m)')
ax.legend(fontsize=8); ax.grid(True, alpha=0.3)
ax.set_ylim(-0.1, 1.8)

# --- 子图 3：改变 Ki（固定 Kp=10, Kd=3） ---
ax = axes[2]
for Ki in [0, 2, 8, 20]:
    z_hist = simulate_pid(Kp=10.0, Kd=3.0, Ki=Ki, dt=dt, T=T)
    ax.plot(t, z_hist, label=f'Ki={Ki}')
ax.axhline(y=1.0, color='k', ls='--', alpha=0.4, label='目标')
ax.set_title('改变 Ki（Kp=10, Kd=3）')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('高度 z (m)')
ax.legend(fontsize=8); ax.grid(True, alpha=0.3)
ax.set_ylim(-0.1, 1.8)

plt.suptitle('PID 参数对无人机高度阶跃响应的影响', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control4.png', dpi=150, bbox_inches='tight')
plt.show()

Execution result:

5. Model-based Control

5.1 Second-Order System Model

A common second-order system model:

Here, $m$ represents the quality, $b$ is the damping coefficient, $k$ is the spring stiffness, and $u(t)$ is the control input.

5.2 PD Control

PD control law:

Among them, $e(t) = x^{des}(t) - x(t)$ represents the tracking error.

Personal understanding: Pure PD control does not utilize model information—it cannot fully compensate for system dynamics. With sensor noise and model errors, tracking error will inevitably occur. This is why model-based feedforward control is introduced.

5.3 Model-based Control Law (Feedforward + PD Feedback)

Personal understanding: The structure of this control law = feedforward term ($\hat{m}\ddot{x}^{des} + \hat{b}\dot{x} + \hat{k}x$, using the model to cancel known dynamics) + PD feedback term ($k_d\dot{e} + k_pe$, correcting modeling errors and disturbances).

Personal understanding: Compared to pure PD control, model-based control laws include two feedforward compensations: $\hat{b}\dot{x}$ and $\hat{k}x$. The feedforward terms use model knowledge to actively counteract known dynamics, while PD feedback is responsible for correcting modeling errors. $\hat{m}, \hat{b}, \hat{k}$ represents the model estimate—the more accurate the estimate, the less the burden on the feedback term.

5.4 Model-based Control Steps

1. Obtain the dynamics of the target system
2. Specific sub-models

Servo-based control:

• Use a PID or PD controller with differentiation to converge the error to 0
• Independent subsystem model

Personal understanding: Both PID and LQR require a linearized model as a prerequisite. The nonlinear system is linearized at operating points (such as hovering points), and controllers are designed for each linearized subsystem separately.

5.5 Control Block Diagram (2D Case)

ydes, zdes    ┌──────────┐     ┌──────┐     ┌──────────┐   u₂   ┌─────┐
ẏdes, żdes ──>│ 位置控制  │────>│ φ_c  │────>│ 姿态控制  │──────>│ UAV │
              └──────────┘     └──────┘     └──────────┘       └─────┘
                   ↑               u₁                             │
                   └──── y, z, ẏ, ż ──────── φ, φ̇ ──────────────┘

5.6 Equilibrium Conditions and Linearization

Balance condition (hover state):

Linearized dynamics model (near the hover point):

Personal understanding: After linearization, $y$ and $\phi$ are directly coupled ($\ddot{y} = -g\phi$), $z$ is independently controlled by $u_1$, and $\phi$ is independently controlled by $u_2$. Thus, the original 6-dimensional nonlinear system becomes two independent linear subsystems: (1) Z-axis height control, and (2) position-pose control coupled with Y-$\phi$.

5.7 Z-axis Direction Position Control

PD control law:

Substitute model $\ddot{z} = -g + \frac{u_1}{m}$:

5.8 Y-axis Direction Position Control

Outer Loop——PD Position Control Law:

Using the model $\ddot{y} = -g\phi$, the desired roll angle (outer loop output) is obtained:

Inner Loop——PD Attitude Control:

Personal understanding: The Y-axis position control is a typical cascade control structure—the outer loop position controller outputs the desired roll angle $\phi_c$, and the inner loop attitude controller tracks this desired angle. The inner loop frequency must be higher than that of the outer loop (usually 5~10 times) to ensure proper time scale separation.

Python Validation

pythonimport numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== 2D 无人机（Y-Z 平面）前馈 + PD 控制仿真 ==========

# --- 物理参数 ---
m = 0.5          # 质量 (kg)
g = 9.81         # 重力加速度 (m/s^2)
Ixx = 0.002      # 绕 x 轴转动惯量 (kg·m^2)
dt = 0.002       # 仿真步长 (s)
T_sim = 6.0      # 总仿真时间
N = int(T_sim / dt)

# --- PD 增益 ---
kp_z, kd_z = 15.0, 8.0       # Z 轴位置 PD
kp_y, kd_y = 8.0, 6.0        # Y 轴位置 PD
kp_phi, kd_phi = 150.0, 20.0 # 姿态角 PD

# --- 期望轨迹：悬停 2s 后移动到目标位置 ---
y_des = np.where(np.arange(N) * dt < 2.0, 0.0, 2.0)
z_des = np.where(np.arange(N) * dt < 2.0, 1.0, 2.5)

# --- 状态初始化 [y, z, phi, dy, dz, dphi] ---
state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
hist_y, hist_z, hist_u1, hist_u2, hist_phi = [], [], [], [], []

for i in range(N):
    y, z, phi, dy, dz, dphi = state

    # 外环：Z 轴 PD + 重力前馈
    ddz_c = kp_z * (z_des[i] - z) + kd_z * (0 - dz)
    u1 = m * (g + ddz_c) / (np.cos(phi) + 1e-6)  # 前馈：mg/cos(φ)
    u1 = np.clip(u1, 0, 3 * m * g)  # 推力限幅

    # 外环：Y 轴 PD → 期望滚转角
    ddy_c = kp_y * (y_des[i] - y) + kd_y * (0 - dy)
    phi_c = -ddy_c / g  # 由线性化关系 ÿ = -g·φ
    phi_c = np.clip(phi_c, -0.5, 0.5)  # 限制期望角度

    # 内环：姿态 PD
    u2 = Ixx * (kp_phi * (phi_c - phi) + kd_phi * (0 - dphi))

    # 非线性动力学更新
    ddy = -(u1 / m) * np.sin(phi)
    ddz = (u1 / m) * np.cos(phi) - g
    ddphi = u2 / Ixx

    state += np.array([dy, dz, dphi, ddy, ddz, ddphi]) * dt

    hist_y.append(y); hist_z.append(z)
    hist_u1.append(u1); hist_u2.append(u2); hist_phi.append(phi)

t_arr = np.arange(N) * dt

# --- 绘图：2 行 2 列 ---
fig, axes = plt.subplots(2, 2, figsize=(13, 9))

# Y-Z 轨迹
axes[0, 0].plot(hist_y, hist_z, 'b-', lw=1.5, label='实际轨迹')
axes[0, 0].plot(y_des[0], z_des[0], 'go', ms=10, label='起始目标')
axes[0, 0].plot(y_des[-1], z_des[-1], 'r*', ms=14, label='终点目标')
axes[0, 0].set_xlabel('Y (m)'); axes[0, 0].set_ylabel('Z (m)')
axes[0, 0].set_title('Y-Z 平面飞行轨迹'); axes[0, 0].legend(); axes[0, 0].grid(True, alpha=0.3)

# 高度与水平位置响应
axes[0, 1].plot(t_arr, hist_z, 'b-', label='z 实际')
axes[0, 1].plot(t_arr, z_des, 'b--', alpha=0.5, label='z 期望')
axes[0, 1].plot(t_arr, hist_y, 'r-', label='y 实际')
axes[0, 1].plot(t_arr, y_des, 'r--', alpha=0.5, label='y 期望')
axes[0, 1].set_xlabel('时间 (s)'); axes[0, 1].set_ylabel('位置 (m)')
axes[0, 1].set_title('位置跟踪响应'); axes[0, 1].legend(); axes[0, 1].grid(True, alpha=0.3)

# 控制输入
axes[1, 0].plot(t_arr, hist_u1, 'g-', label='u₁ (总推力)')
axes[1, 0].axhline(y=m * g, color='k', ls='--', alpha=0.4, label=f'悬停推力 mg={m*g:.2f}')
axes[1, 0].set_xlabel('时间 (s)'); axes[1, 0].set_ylabel('推力 (N)')
axes[1, 0].set_title('总推力 u₁'); axes[1, 0].legend(); axes[1, 0].grid(True, alpha=0.3)

# 滚转角
axes[1, 1].plot(t_arr, np.degrees(hist_phi), 'm-', label='φ (滚转角)')
axes[1, 1].set_xlabel('时间 (s)'); axes[1, 1].set_ylabel('角度 (°)')
axes[1, 1].set_title('滚转角 φ 响应'); axes[1, 1].legend(); axes[1, 1].grid(True, alpha=0.3)

plt.suptitle('第五章：2D 无人机前馈 + PD 控制仿真（悬停→移动）', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control5.png', dpi=150, bbox_inches='tight')
plt.show()

Execution result:

VI. Three-Dimensional Trajectory Tracking Control

6.1 Basic Framework for Trajectory Tracking

Provide the desired trajectory $\mathbf{x}^{des}(t)$, $\dot{\mathbf{x}}^{des}(t)$, and $\ddot{\mathbf{x}}^{des}(t)$, and track it within the time $t \in [t_0, T]$.

Definition error:

Control law:

6.2 3D Control Diagram

p_des  ┌────────────┐   ┌──────────┐        ┌──────────┐  u₂,u₃,u₄  ┌─────┐
ṗ_des ─>│ 轨迹跟踪    │──>│ 位置控制  │──u₁──>│ 姿态控制  │──────────>│ UAV │
       └────────────┘   └──────────┘  φc,θc  └──────────┘           └─────┘
            ↑                                                          │
            └─── p, ṗ ──────────────── φ,θ,ψ, ωx,ωy,ωz ─────────────┘

6.3 Dynamic Equations in the Body Coordinate System

Newton’s Equation:

Euler’s Equation:

Angular velocity recovery: $\omega_b = R^T \dot{R}$

6.4 ZXY Rotation Euler Angle Expansion

Where $p$ = Roll angular velocity, $q$ = Pitch angular velocity, and $r$ = Yaw angular velocity.

Personal understanding: This angular velocity conversion matrix maps the Euler angle derivatives (defined in each intermediate rotation coordinate system) to the angular velocity components in the spacecraft coordinate system.

6.5 Euler Angle Differential Equations

Inverse relationship of Euler angle differentiation (to solve $\dot{\phi}, \dot{\theta}, \dot{\psi}$):

6.6 Hover Point Linearization

Linearize near the hover points ($\psi_0 = 0$, $\theta_0 = 0$, $u_{1,0} = mg$):

Personal understanding: Note that $\psi$ is not zero (i.e., the yaw angle is not zero). The accelerations in the directions of $x$ and $y$ both depend on $\phi$ and $\theta$ simultaneously. This is the coupling caused by the yaw angle. When $\psi = 0$ occurs, it simplifies to $\ddot{x} = g\theta$, $\ddot{y} = -g\phi$, which is consistent with the 2D situation in Chapter 5.

6.7 Position Controller (PID)

Total thrust:

Expected attitude angle (considering yaw angle $\psi$):

Personal understanding: The position controller only outputs the desired $\phi_c, \theta_c$ and thrust $u_1$. The yaw angle $\psi$ is tracked by a separate yaw controller according to the expected value $\psi^{des}$, and does not participate in the external position loop.

6.8 Pose Controller (PID)

Control Moment:

Personal understanding: The second term $\omega \times I\omega$ in the torque control law is a feedforward compensation term used to counteract the gyroscopic effect. It can be ignored near hover, but must be included during rapid maneuvering.

Python Validation

pythonimport numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== 3D "8"字轨迹跟踪仿真 ==========

# --- 物理参数 ---
m = 0.5; g = 9.81; dt = 0.005; T_sim = 12.0
N = int(T_sim / dt)
t = np.arange(N) * dt
Kp = np.diag([6.0, 6.0, 10.0])    # 位置 PD 增益
Kd = np.diag([4.0, 4.0, 6.0])
kp_att, kd_att = 80.0, 15.0       # 姿态 PD 增益
Ixx = Iyy = 0.002; Izz = 0.004

# --- 期望 "8" 字轨迹 ---
omega_t = 2 * np.pi / T_sim
x_des = 2.0 * np.sin(omega_t * t)
y_des = 1.5 * np.sin(2 * omega_t * t)
z_des = 1.0 + 0.3 * np.sin(omega_t * t)

# 期望速度、加速度（解析求导）
dx_des = 2.0 * omega_t * np.cos(omega_t * t)
dy_des = 1.5 * 2 * omega_t * np.cos(2 * omega_t * t)
dz_des = 0.3 * omega_t * np.cos(omega_t * t)
ddx_des = -2.0 * omega_t**2 * np.sin(omega_t * t)
ddy_des = -1.5 * (2*omega_t)**2 * np.sin(2 * omega_t * t)
ddz_des = -0.3 * omega_t**2 * np.sin(omega_t * t)

# --- 状态初始化 ---
pos = np.array([0.0, 0.0, 0.0])   # 位置
vel = np.array([0.0, 0.0, 0.0])   # 速度
phi, theta, psi = 0.0, 0.0, 0.0   # 欧拉角
dphi, dtheta, dpsi = 0.0, 0.0, 0.0

hist_pos = np.zeros((N, 3))
for i in range(N):
    p_des = np.array([x_des[i], y_des[i], z_des[i]])
    v_des = np.array([dx_des[i], dy_des[i], dz_des[i]])
    a_des = np.array([ddx_des[i], ddy_des[i], ddz_des[i]])

    # 位置 PD 控制 → 期望加速度
    ep = p_des - pos; ev = v_des - vel
    acc_cmd = a_des + Kp @ ep + Kd @ ev

    # 总推力（前馈 + PD）
    u1 = m * np.sqrt(acc_cmd[0]**2 + acc_cmd[1]**2 + (acc_cmd[2] + g)**2)
    u1 = np.clip(u1, 0.1, 4 * m * g)

    # 期望姿态角（考虑 ψ≈0 简化）
    phi_c = np.clip((acc_cmd[0]*np.sin(psi) - acc_cmd[1]*np.cos(psi)) / g, -0.5, 0.5)
    theta_c = np.clip((acc_cmd[0]*np.cos(psi) + acc_cmd[1]*np.sin(psi)) / g, -0.5, 0.5)

    # 姿态 PD 控制
    u2 = Ixx * (kp_att * (phi_c - phi) + kd_att * (0 - dphi))
    u3 = Iyy * (kp_att * (theta_c - theta) + kd_att * (0 - dtheta))

    # 简化 3D 动力学更新
    cphi, sphi = np.cos(phi), np.sin(phi)
    cth, sth = np.cos(theta), np.sin(theta)
    cpsi, spsi = np.cos(psi), np.sin(psi)
    ax = (u1/m) * (cpsi*sth*cphi + spsi*sphi)
    ay = (u1/m) * (spsi*sth*cphi - cpsi*sphi)
    az = (u1/m) * cphi*cth - g

    vel += np.array([ax, ay, az]) * dt
    pos += vel * dt
    dphi += (u2 / Ixx) * dt; phi += dphi * dt
    dtheta += (u3 / Iyy) * dt; theta += dtheta * dt

    hist_pos[i] = pos

# --- 绘图：3D 轨迹对比 ---
fig = plt.figure(figsize=(12, 8))
ax3d = fig.add_subplot(1, 1, 1, projection='3d')
ax3d.plot(x_des, y_des, z_des, 'b--', lw=1.5, alpha=0.6, label='期望轨迹（"8"字）')
ax3d.plot(hist_pos[:, 0], hist_pos[:, 1], hist_pos[:, 2],
          'r-', lw=1.2, label='实际轨迹')

# 标注起点和终点
ax3d.scatter(*hist_pos[0], c='g', s=100, marker='o', zorder=5, label='起点')
ax3d.scatter(*hist_pos[-1], c='k', s=100, marker='*', zorder=5, label='终点')

# 每隔一段绘制位置标记
step = N // 10
for j in range(0, N, step):
    ax3d.plot([x_des[j], hist_pos[j, 0]], [y_des[j], hist_pos[j, 1]],
              [z_des[j], hist_pos[j, 2]], 'k-', alpha=0.2, lw=0.8)

ax3d.set_xlabel('X (m)'); ax3d.set_ylabel('Y (m)'); ax3d.set_zlabel('Z (m)')
ax3d.set_title('第六章：3D "8"字轨迹跟踪（PD 控制）', fontsize=14)
ax3d.legend(fontsize=10)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control6.png', dpi=150, bbox_inches='tight')
plt.show()

Execution result:

VII. SE(3) Control

Personal understanding: When the approximation error caused by Euler angle linearization is unacceptable (e.g., for large angular maneuvers), SE(3) control should be used—it defines the error directly on the rotation matrix without any linearization approximations, thus preserving precision.

7.1 Tracking Control on the SE(3) Group

SE(3) control directly defines the error on the special Euclidean group $SE(3)$, avoiding the Gimbal Lock problem of Euler angles.

Status Representation:

Here, $R \in SO(3)$ is the rotation matrix, and $\mathbf{p} \in \mathbb{R}^3$ is the position.

7.2 Error Definition

Position error:

Speed error:

7.3 Expected Force Calculation

Personal understanding: The desired force is projected onto the body $z$ axis to obtain the total thrust scalar $u_1$. The remaining force direction information is used to construct the desired posture. Thus, position control and posture control remain decoupled: the position loop outputs the desired force, and the posture loop tracks the desired posture.

Total thrust (projected onto the body $z$ axis):