Overview
This demo implements Model Predictive Control (MPC) for autonomous drone racing
using Sequential Quadratic Programming (SQP). The controller predicts 500ms into the future
and optimizes thrust and angular rate commands in real-time.
Coordinate system: Y-up (Three.js convention)
1. System Model
State Vector (14-dimensional):
x = [px, py, pz, // position
vx, vy, vz, // velocity
qw, qx, qy, qz, // quaternion
T, ωr, ωp, ωy] // actuators
Input Vector (4-dimensional):
u = [Tcmd, ωr,cmd, ωp,cmd, ωy,cmd]
2. Continuous Dynamics
Position: ṗ = v
Velocity (with drag):
v̇ = (1/m)·R(q)·[0, T, 0]ᵀ - [0, g, 0]ᵀ - cd·v
Quaternion kinematics:
q̇ = ½ · q ⊗ [0, ωbody]
// Full quaternion product expansion:
q̇w = -½(qxωx + qyωy + qzωz)
q̇x = ½(qwωx + qyωz - qzωy)
q̇y = ½(qwωy + qzωx - qxωz)
q̇z = ½(qwωz + qxωy - qyωx)
Actuator dynamics (first-order):
Ṫ = (Tcmd - T) / τT
ω̇ = (ωcmd - ω) / τω
τT = 40ms, τω = 30ms
3. MPC Formulation
Optimal control problem solved at each timestep:
min Σk=0N-1 ‖xk - xkref‖Q² + ‖uk - ukref‖R²
+ terminal cost ‖xN - xNref‖Qf²
uref from feedforward (hover thrust + trajectory acceleration)
Subject to:
• xk+1 = F(xk, uk, Δt) // discrete dynamics
• x0 = xcurrent // initial condition
• umin ≤ uk ≤ umax // box constraints
4. MPC Configuration
Horizon
N=10 × 50ms = 500ms
Position weight Qp
400
Velocity weight Qv
0.1
Attitude weight Qθ
5
Input weight R
0.1 · I4
Input Bounds:
Thrust
[0, 50] m/s² (5:1 T/W)
Roll/Pitch rate
±20 rad/s
Yaw rate
±10 rad/s
5. Sequential Quadratic Programming
Linearize discrete dynamics at operating point (x̄k, ūk):
xk+1 ≈ Akxk + Bkuk + ck
// Jacobians via finite differences (ε = 10⁻⁶)
A = ∂F/∂x (14×14)
B = ∂F/∂u (14×4)
Condensed QP formulation eliminates states:
xk = Φkx0 + ΨkΔU + dk
ΔU ∈ ℝN·nu = ℝ40
6. QP Solver
Projected Gradient with Nesterov Acceleration:
min ½ΔUᵀHΔU + gᵀΔU
s.t. lb ≤ ΔU ≤ ub
Algorithm per iteration:
1. ∇f = H·y + g
2. x̃ = y - α·∇f
3. xk+1 = Π[lb,ub](x̃)
4. β = (tk-1)/tk+1 // momentum
5. y = xk+1 + β(xk+1 - xk)
Step size α = 1/λmax via Gershgorin circles. QP dim: 40×40.
7. Control Pipeline
// ~60Hz control loop
1. Sample trajectory waypoints
2. Compute feedforward (accel → quat)
3. Initialize trajectory rollout
4. SQP iteration:
a. Linearize dynamics
b. Build condensed QP (Ψ matrices)
c. Solve via projected gradient
d. Update trajectory
5. Extract u0 = unom + Δu0
6. Apply to drone dynamics
8. Potential Improvements
Numerical Jacobians
56 evals/linearization
→ Analytical Jacobians: 5-10× speedup
Projected Gradient
~50 iterations
→ OSQP/qpOASES: 5-20 iterations
Warm-start disabled
circular issues
→ Fix for faster convergence
Controls
Mouse drag
Orbit camera
Scroll
Zoom in/out
Overview/Follow
Camera modes
Code References
MPC.ts
- SQP solver, QP formulation
MPCModel.ts
- dynamics, linearization
QPSolver.ts
- projected gradient
DroneDynamics.ts
- simulation model