betaFeedback

WorksMathematics

OCP-Based Nonlinear Model Predictive Control for Quadrotor Trajectory Tracking

Abstract

Fast online optimization is essential for nonlinear model predictive control (NMPC) on resource-limited aerial platforms. This paper presents an analytical-derivative NMPC solver for quadrotor trajectory tracking. For the state-affine translational model with held thrust and attitude commands, fourth-order Runge–Kutta integration yields an exact discrete-time prediction map. Forward–backward difference equations provide the gradient and exact Hessian of the finite-horizon cost condensed over the input sequence. These derivatives drive a nested optimal-control-based direction recursion, while a shifted warm start reuses the preceding solution. The resulting solver addresses the unconstrained tracking problem without a general-purpose nonlinear programming package. Gazebo experiments show comparable tracking accuracy to iLQR and two acados SQP configurations, with mean solver-time speedups of 1.971.97–2.302.30. In onboard flight tests, the mean solver time is 0.31210.3121,ms, corresponding to speedups of 3.373.37–5.775.77 over the same baselines. The largest measured solver time is 3.77543.7754,ms within a 2020-ms control period. These measurements cover the numerical solver and exclude estimation, communication, and command publication.

Introduction

UNMANNED aerial vehicles (UAVs) are used for inspection, mapping, and aerial manipulation, where accurate tracking of three-dimensional trajectories is often required [1]. Their underactuated and nonlinear dynamics make nonlinear model predictive control (NMPC) an attractive tracking framework [2], [3]. By solving a finite-horizon optimal control problem at each sampling instant and applying its first input, NMPC accounts for predicted tracking performance as the trajectory evolves. Its effectiveness has been demonstrated in agile flight and disturbance-rejection tracking [4, 5, 6, 7, 8].

Onboard NMPC typically operates under tight time limits for each finite-horizon solve [9]. Continuation/generalized minimal residual methods reduce the work per sample by updating the Karush–Kuhn–Tucker conditions along the sampling sequence [10]. Short stabilizing horizons, state-dependent-coefficient reformulations, and code generation for microcontrollers reduce the problem size or replace the nonlinear program with simpler subproblems [11, 12, 13]. Analytical Koopman lifting leads to a quadratic program [14], whereas event-triggered NMPC avoids solving the optimization problem at every sampling instant [15]. Although these approaches substantially improve real-time feasibility, their computational savings are generally obtained through problem reformulation, model approximation, or fewer optimization updates.

A complementary route to real-time NMPC is to reduce derivative and optimization costs while retaining the nonlinear prediction model. The finite-horizon NMPC problem solved at each sampling instant can be viewed in two ways. The static viewpoint casts it as a nonlinear program and uses methods such as interior-point, quasi-Newton, or sequential quadratic programming (SQP), with derivatives obtained numerically or by automatic differentiation [16, 17, 18, 19]. This is flexible but can introduce software and computational overhead. The dynamic viewpoint derives forward and backward difference equations (FBDEs) from Pontryagin’s maximum principle and computes derivatives through state and costate recursions. It avoids a general-purpose nonlinear programming package, although conventional first-order updates can converge slowly [20].

A recent line of work unifies the two viewpoints. Building on the optimization framework rooted in optimal control of [21], which develops a superlinearly convergent algorithm from an optimal-control reformulation, the OCP methods in [22], [23] compute the cost gradient by solving FBDEs, and the subsequent work [24] introduces a virtual optimal-control problem to derive a second set of FBDEs for Hessian evaluation. The latter delivers a complete second-order solver that does not rely on optimization packages and was validated on a linear–quadratic regulator and a planar automatic guided vehicle, where it matched the accuracy of mature solvers while reducing computation time by roughly an order of magnitude. That solver, however, retains the first-order forward-Euler system model, because higher-order discretization of general nonlinear dynamics makes the associated derivative recursions substantially more involved. The resulting O(h)O(h) global integration error may therefore limit the attainable prediction and tracking accuracy.

The quadrotor dynamics are affine in the state. With the input held constant over a sampling interval, fourth-order Runge–Kutta (RK4) gives the exact discrete-time model while preserving FBDE recursions. The resulting gradient and exact Hessian are used by the OCP nested direction recursion to update the input sequence. The contributions are as follows.

This work was supported by the International (Regional) Cooperation and Exchange Program of the National Natural Science Foundation of China (W2611094), the Original Exploratory Program Project of National Natural Science Foundation of China (62450004), the Joint Funds of the National Natural Science Foundation of China (U23A20325), and Key R&D Program of Shandong Province of China (2025CХPT099). (Corresponding author: Huanshui Zhang.)

Shichao Lv, Chuanzhi Lv, Hongxia Wang, and Huanshui Zhang are with the College of Electrical Engineering and Automation, Shandong University of Science and Technology, Qingdao 266590, China (e-mail: Shichao Lv, [email protected]; Chuanzhi Lv, [email protected]; Hongxia Wang, [email protected]; Huanshui Zhang, [email protected]).

Huanshui Zhang is also with the School of Control Science and Engineering, Shandong University, Jinan, Shandong 250061, China.

All authors are also with the Shandong Provincial Key Laboratory of Intelligent Sensing and Autonomous Control.

  1. For real-time UAV trajectory tracking, an RK4-discretized model is combined with FBDE recursions to obtain the analytical gradient and exact Hessian of the finite-horizon cost with respect to the input sequence. The OCP nested direction recursion uses these derivatives without a general-purpose optimization package.

  1. A shifted warm start uses the preceding input sequence to initialize the optimization at each sampling instant. The applied input is discarded, the remaining inputs move forward one stage, and the terminal reference input fills the last stage. This reduces online iterations and solution time.

  1. Gazebo and real-flight tests quantify the computational performance. In the Gazebo comparison against the iterative linear quadratic regulator (iLQR), acados SQP with the high-performance interior-point method (acados SQP+HPIPM), and acados SQP with the online active-set QP solver (acados SQP+qpOASES), Algorithm 1 achieves a mean solution time of 0.1197 ms, which is 1.97–2.30 times faster than the baseline methods at comparable tracking accuracy and with no recorded failures. The corresponding real-flight mean is 0.3121 ms, giving speedup factors of 3.37–5.77. The largest measured time is 3.7754 ms, leaving sufficient solver-time margin for the 20-ms control period.

Figure 1 shows how state estimation, the proposed solver, and control execution are connected.

Controller architecture

Figure 1. Controller architecture. The NMPC solver combines the exact RK4-discretized model, FBDE derivatives, the OCP update, and a shifted warm start.

The remainder of this article is organized as follows. The quadrotor system model, its exact discrete-time representation, and the finite-horizon NMPC problem are presented in Section II. The FBDE-based gradient and Hessian recursions, together with the OCP solver, are developed in Section III. Gazebo validation and real-flight comparisons are reported in Sections IV and V, respectively. Finally, conclusions are drawn in Section VI.

Notation: Rn\mathbb{R}^{n} denotes the nn-dimensional Euclidean space. The symbols (⋅)⊤(\cdot)^{\top} and ∥⋅∥\|\cdot\| denote transposition and the Euclidean norm, respectively, while 030_{3} and I3I_{3} denote the 3×33 \times3 zero and identity matrices.

Problem Formulation

This section develops the discrete translational model used for prediction. It then formulates the finite-horizon NMPC problem and eliminates the predicted states to leave the input sequence as the decision variable.

Model and Exact Discretization

The coordinate convention is shown in Figure 2, where p=[x,y,z]⊤\mathbf{p}=[x,y,z]^{\top}, v=[vx,vy,vz]⊤\mathbf{v}=[v_x,v_y,v_z]^{\top}, and U=[T,ϕ,θ]⊤\mathbf{U}=[T,\phi,\theta]^{\top} denote the position, velocity, and control input, respectively. Here, TT is the total thrust, while ϕ\phi and θ\theta are the commanded angles tracked by the faster attitude loop. With yaw regulated independently, the dynamics are

Coordinate frames and variables of the experimental quadrotor

Figure 2. Coordinate frames and variables of the experimental quadrotor. OIO_I and ObO_b denote the origins of the inertial and body-fixed frames, respectively. The total thrust acts along zbz_b; ϕ\phi and θ\theta denote rotations about xbx_b and yby_b, respectively.

{x¨=Tmsin⁡θcos⁡ϕ,y¨=−Tmsin⁡ϕ,z¨=Tmcos⁡θcos⁡ϕ−g,(1)\begin{cases} \ddot{x}=\dfrac{T}{m}\sin\theta\cos\phi,\\ \ddot{y}=-\dfrac{T}{m}\sin\phi,\\ \ddot{z}=\dfrac{T}{m}\cos\theta\cos\phi-g, \tag*{(1)} \end{cases}

where mm is the quadrotor mass and gg is the gravitational acceleration.

Defining the state and input vectors as

X=[x,y,z,vx,vy,vz]⊤∈R6,U=[T,ϕ,θ]⊤∈R3,X=[x,y,z,v_x,v_y,v_z]^{\top}\in\mathbb{R}^{6},\quad U=[T,\phi,\theta]^{\top}\in\mathbb{R}^{3},

the dynamics in (1) can be written as

X˙=f(X,U)=AcX+b(U),(2)\dot{X}=f(X,U)=A_cX+b(U), \tag*{(2)}

where

Ac=[03I30303],b(U)=[000Tmsin⁡θcos⁡ϕ−Tmsin⁡ϕTmcos⁡θcos⁡ϕ−g].A_c=\begin{bmatrix} 0_3&I_3\\ 0_3&0_3 \end{bmatrix},\quad b(U)=\begin{bmatrix} 0\\ 0\\ 0\\ \dfrac{T}{m}\sin\theta\cos\phi\\ -\dfrac{T}{m}\sin\phi\\ \dfrac{T}{m}\cos\theta\cos\phi-g \end{bmatrix}.

The drift matrix in (2) satisfies Ac2=0A_c^2=0. Let h>0h>0 be the prediction interval. Holding UkU_k constant over [kh,(k+1)h)[kh,(k+1)h) also makes b(Uk)b(U_k) constant, and the RK4 stages give

F(Xk,Uk)=[xk+hvx,k+h2Tk2msin⁡θkcos⁡ϕkyk+hvy,k−h2Tk2msin⁡ϕkzk+hvz,k+h2Tk2mcos⁡θkcos⁡ϕk−h22gvx,k+hTkmsin⁡θkcos⁡ϕkvy,k−hTkmsin⁡ϕkvz,k+hTkmcos⁡θkcos⁡ϕk−hg].(3)F(X_k,U_k)=\begin{bmatrix} x_k+h v_{x,k}+\dfrac{h^2T_k}{2m}\sin\theta_k\cos\phi_k\\ y_k+h v_{y,k}-\dfrac{h^2T_k}{2m}\sin\phi_k\\ z_k+h v_{z,k}+\dfrac{h^2T_k}{2m}\cos\theta_k\cos\phi_k-\dfrac{h^2}{2}g\\ v_{x,k}+\dfrac{hT_k}{m}\sin\theta_k\cos\phi_k\\ v_{y,k}-\dfrac{hT_k}{m}\sin\phi_k\\ v_{z,k}+\dfrac{hT_k}{m}\cos\theta_k\cos\phi_k-hg \end{bmatrix}. \tag*{(3)}

The same update can be written more compactly as

pk+1=pk+hvk+h22a(Uk),(4)p_{k+1}=p_k+hv_k+\dfrac{h^2}{2}a(U_k), \tag*{(4)}
vk+1=vk+ha(Uk),(5)v_{k+1}=v_k+ha(U_k), \tag*{(5)}

where the acceleration vector is

a(Uk)=[ax,k,ay,k,az,k]⊤,a(U_k)=[a_{x,k},a_{y,k},a_{z,k}]^{\top},
ax,k=Tkmsin⁡θkcos⁡ϕk,a_{x,k}=\dfrac{T_k}{m}\sin\theta_k\cos\phi_k,
ay,k=−Tkmsin⁡ϕk,a_{y,k}=-\dfrac{T_k}{m}\sin\phi_k,
az,k=Tkmcos⁡θkcos⁡ϕk−g.a_{z,k}=\dfrac{T_k}{m}\cos\theta_k\cos\phi_k-g.

Because a(Uk)a(U_k) is constant over the interval, velocity changes linearly and integration adds h2a(Uk)/2h^2a(U_k)/2 to the position update. Thus, (3) is the exact discrete-time solution of (2) under held inputs. Forward Euler gives the same velocity update but omits this acceleration-induced displacement, causing an O(h2)O(h^2) one-step position error.

NMPC Formulation

For trajectory tracking, let (Xkr,Ukr)(X_k^r, U_k^r) denote the reference sequences over a horizon of NN stages. The reference input UkrU_k^r is obtained from the reference acceleration using the standard differential-flatness relation for quadrotors [1]. The corresponding tracking errors are Xke=Xk−XkrX_k^e = X_k - X_k^r and Uke=Uk−UkrU_k^e = U_k - U_k^r. Collecting the input sequence as U=[U0⊤,…,UN−1⊤]⊤∈R3N\mathcal{U} = [U_0^\top,\ldots,U_{N-1}^\top]^\top\in\mathbb{R}^{3N} gives the following NMPC problem:

min⁡UJ,J=∑k=0N−1[(Xke)⊤QXke+(Uke)⊤RuUke]+(XNe)⊤QfXNes.t.Xk+1=F(Xk,Uk),X0 given,(6)\begin{aligned} \min_{\mathcal{U}}\quad& J,\\ J =\quad& \sum_{k=0}^{N-1}\left[(X_k^e)^\top QX_k^e+(U_k^e)^\top R_uU_k^e\right]+(X_N^e)^\top Q_fX_N^e\\ \text{s.t.}\quad& X_{k+1}=F(X_k,U_k),\quad X_0\ \text{given}, \tag*{(6)} \end{aligned}

where Q⪰0Q \succeq0, Qf⪰0Q_f \succeq0, Ru≻0R_u \succ0, and FF is given by (3). Repeated application of the system model gives

Xk=Ψk(X0,U0,…,Uk−1),(7)X_k=\Psi_k(X_0,U_0,\ldots,U_{k-1}), \tag*{(7)}

where Ψk\Psi_k denotes the resulting kk-step system model. Substituting (7) into the cost (6) yields

min⁡U∈R3NJ(X0,U).(8)\min_{\mathcal{U}\in\mathbb{R}^{3N}} J(X_0,\mathcal{U}). \tag*{(8)}

Eq. (8) is the problem solved at each sampling instant. Its only decision variable is the physical input sequence U\mathcal{U}; the state evolution remains embedded in Ψk\Psi_k. This form makes it possible to update the complete sequence using derivatives of JJ with respect to U\mathcal{U}.

Solver and Analytical Derivatives

To solve (8), the OCP iteration requires the gradient and Hessian of J(X0,U)J(X_0,\mathcal{U}). The update is stated first, followed by compact FBDE expressions, the shifted warm start, and the complete online algorithm.

OCP Iterative Update

At optimization iteration ii, the current input sequence Ui\mathcal{U}_i is updated by

Ui+1=Ui−di(Ui),(9)\mathcal{U}_{i+1}=\mathcal{U}_i-d_i(\mathcal{U}_i), \tag*{(9)}

where di(Ui)d_i(\mathcal{U}_i) denotes the update vector computed from the derivatives of J(X0,U)J(X_0,\mathcal{U}).

The OCP method in [21], [24] constructs this update recursively using the gradient and Hessian. For the present NMPC problem, the cost, gradient, and Hessian at Ui\mathcal{U}_i are denoted by

Ji=J(X0,Ui),J_i=J(X_0,\mathcal{U}_i),
∇Ji=∇UJ(X0,Ui),\nabla J_i=\nabla_{\mathcal{U}}J(X_0,\mathcal{U}_i),
Hi=∇U2J(X0,Ui).H_i=\nabla_{\mathcal{U}}^2J(X_0,\mathcal{U}_i).

For a fixed matrix R≻0\mathcal{R}\succ0, the search direction is generated by

d0(Ui)=(R+Hi)−1∇Ji,(10)d_0(\mathcal{U}_i)=(\mathcal{R}+H_i)^{-1}\nabla J_i, \tag*{(10)}
dj(Ui)=(R+Hi)−1(∇Ji+Rdj−1(Ui)),j=1,…,i,(11)d_j(\mathcal{U}_i)=(\mathcal{R}+H_i)^{-1}\left(\nabla J_i+\mathcal{R}d_{j-1}(\mathcal{U}_i)\right),\quad j=1,\ldots,i, \tag*{(11)}
Ui+1=Ui−di(Ui).(12)\mathcal{U}_{i+1}=\mathcal{U}_i-d_i(\mathcal{U}_i). \tag*{(12)}

Eq. (10) gives the initial direction. It is refined ii times by (11), after which (12) advances the input sequence. Thus the inner recursion grows with the outer iteration index. Since R+Hi\mathcal{R}+H_i remains fixed during these refinements, its inverse is computed once per outer iteration and reused.

The required ∇Ji\nabla J_i and HiH_i are derived next. The gradient follows from the state and costate equations; differentiating these recursions gives the Hessian.

Analytical Gradient

After forward prediction with (3), define Ek=∂F/∂UkE_k=\partial F/\partial U_k and Ad=∂F/∂X=[I3I3h0I3]A_d=\partial F/\partial X=\begin{bmatrix}I_3&I_3h\\0&I_3\end{bmatrix}. The gradient blocks and backward costate recursion are

γk=2RuUke+Ek⊤λk+1,\gamma_k=2R_uU_k^e+E_k^\top\lambda_{k+1},
λN=2QfXNe,\lambda_N=2Q_fX_N^e,
λk=2QXke+Ad⊤λk+1,k=N−1,…,0.\lambda_k=2QX_k^e+A_d^\top\lambda_{k+1},\quad k=N-1,\ldots,0.

The complete gradient is ∇UJ=[γ0⊤,…,γN−1⊤]⊤\nabla_{\mathcal{U}}J=[\gamma_0^\top,\ldots,\gamma_{N-1}^\top]^\top. For Q=diag⁡(q1,…,q6)Q=\operatorname{diag}(q_1,\ldots,q_6), let Qp=diag⁡(q1,q2,q3)Q_p=\operatorname{diag}(q_1,q_2,q_3) and Qv=diag⁡(q4,q5,q6)Q_v=\operatorname{diag}(q_4,q_5,q_6). Partitioning the costate into position and velocity components gives

λp,k=2QpXp,ke+λp,k+1,\lambda_{p,k}=2Q_pX_{p,k}^e+\lambda_{p,k+1},
λv,k=2QvXv,ke+hλp,k+1+λv,k+1.(13)\lambda_{v,k}=2Q_vX_{v,k}^e+h\lambda_{p,k+1}+\lambda_{v,k+1}. \tag*{(13)}

The remaining term follows directly from the position and velocity updates:

EkTλk+1=h(∂a(Uk)∂Uk)T(λv,k+1+h2λp,k+1).E_k^{\mathsf{T}}\lambda_{k+1}=h\left(\frac{\partial a(U_k)}{\partial U_k}\right)^{\mathsf{T}}\left(\lambda_{v,k+1}+\frac{h}{2}\lambda_{p,k+1}\right).

For compactness, define

μk+1=λv,k+1+h2λp,k+1=[μ4,k+1μ5,k+1μ6,k+1]T.\mu_{k+1}=\lambda_{v,k+1}+\frac{h}{2}\lambda_{p,k+1}=\left[\mu_{4,k+1}\quad\mu_{5,k+1}\quad\mu_{6,k+1}\right]^{\mathsf{T}}.

Thus,

γk=2RuUke+h(∂a(Uk)∂Uk)Tμk+1.\gamma_k=2R_uU_k^e+h\left(\frac{\partial a(U_k)}{\partial U_k}\right)^{\mathsf{T}}\mu_{k+1}.

For Ru=diag⁡(Ru,1,Ru,2,Ru,3)R_u=\operatorname{diag}(R_{u,1},R_{u,2},R_{u,3}), expanding the input derivative gives

\begin{aligned} \gamma_{T,k}&=2R_{u,1}U_{1,k}^e+\frac{h}{m}\left(\sin\theta_k\cos\phi_k\mu_{4,k+1}\\ &\quad-\sin\phi_k\mu_{5,k+1}+\cos\theta_k\cos\phi_k\mu_{6,k+1}\right),\\ \gamma_{\phi,k}&=2R_{u,2}U_{2,k}^e+\frac{h}{m}\left(-\sin\theta_k\sin\phi_k\mu_{4,k+1}\\ &\quad-\cos\phi_k\mu_{5,k+1}-\cos\theta_k\sin\phi_k\mu_{6,k+1}\right),\\ \gamma_{\theta,k}&=2R_{u,3}U_{3,k}^e+\frac{h}{m}\left(\cos\theta_k\cos\phi_k\mu_{4,k+1}\\ &\quad-\sin\theta_k\cos\phi_k\mu_{6,k+1}\right). \tag*{(14)} \end{aligned}

Here, γk=[γT,k,γϕ,k,γθ,k]T\gamma_k=[\gamma_{T,k},\gamma_{\phi,k},\gamma_{\theta,k}]^{\mathsf{T}}. Thus, one forward state evaluation and one backward costate evaluation give the complete gradient, without explicit differentiation of Ψk\Psi_k.

Exact Hessian

The Hessian follows by differentiating the state and costate recursions. For input stage rr, let Sj(r)=∂Xj/∂UrS_j^{(r)}=\partial X_j/\partial U_r and Pj(r)=∂λj/∂UrP_j^{(r)}=\partial\lambda_j/\partial U_r. Since AdA_d is constant, these 6×36\times3 sensitivities satisfy

Sj+1(r)=AdSj(r)+δjrEr,S0(r)=0,S_{j+1}^{(r)}=A_dS_j^{(r)}+\delta_{jr}E_r,\qquad S_0^{(r)}=0,
Pj(r)=2QSj(r)+AdTPj+1(r),PN(r)=2QfSN(r).P_j^{(r)}=2QS_j^{(r)}+A_d^{\mathsf{T}}P_{j+1}^{(r)},\qquad P_N^{(r)}=2Q_fS_N^{(r)}.

Here j=0,…,N−1j=0,\ldots,N-1 and δjr\delta_{jr} is the Kronecker delta. The first recursion is evaluated forward and the second backward. Differentiating γk\gamma_k gives the 3×33\times3 Hessian blocks

Hkr=EkTPk+1(r)+δkr(2Ru+Θk),(15)H_{kr}=E_k^{\mathsf{T}}P_{k+1}^{(r)}+\delta_{kr}(2R_u+\Theta_k), \tag*{(15)}

with the local curvature term

Θk=h∑a=13μa+3,k+1∂2aa(Uk)∂Uk2.(16)\Theta_k=h\sum_{a=1}^{3}\mu_{a+3,k+1}\frac{\partial^2a_a(U_k)}{\partial U_k^2}. \tag*{(16)}

Here aaa_a is component aa of the acceleration vector. The derivatives in (16) are evaluated analytically from the trigonometric expressions in Section II. Because the model has no state–state or state–input second derivatives, (15) includes all second-order terms; retaining Θk\Theta_k distinguishes the exact Hessian from a Gauss–Newton approximation.

For this translational model, the future-state terms in the sensitivity recursions can also be summed in closed form. Define

ca,k=∂[haa(Uk)]∂Uk∈R1×3,a=1,2,3.c_{a,k}=\frac{\partial[ha_a(U_k)]}{\partial U_k}\in\mathbb{R}^{1\times3},\qquad a=1,2,3.

Let Qf=diag⁡(qf,1,…,qf,6)Q_f=\operatorname{diag}(q_{f,1},\ldots,q_{f,6}). For 0≤r≤s≤N−10\le r\le s\le N-1, set M=N−s−1M=N-s-1 and η=s−r\eta=s-r. The MM shared stage states are indexed by ν=0,…,M−1\nu=0,\ldots,M-1, and the terminal state corresponds to ν=M\nu=M. Define

AM,η=M(4M2+6ηM−1)/12,A_{M,\eta}=M(4M^2+6\eta M-1)/12,
BM,η=(M+η+1/2)(M+1/2).B_{M,\eta}=(M+\eta+1/2)(M+1/2).

Direct evaluation of the shared future-state contributions gives

wa,rs=2qah2AM,η+2qa+3M+2qf,ah2BM,η+2qf,a+3,a=1,2,3.(17)w_{a,rs}=2q_ah^2A_{M,\eta}+2q_{a+3}M +2q_{f,a}h^2B_{M,\eta}+2q_{f,a+3},\qquad a=1,2,3. \tag*{(17)}

For r≤sr\le s, the corresponding exact Hessian blocks are

Hrs=∑a=13wa,rsca,rca,s+{0,r<s,2Ru+Θr,r=s,(18)H_{rs}=\sum_{a=1}^{3}w_{a,rs}c_{a,r}c_{a,s}+\begin{cases} 0,&r<s,\\ 2R_u+\Theta_r,&r=s, \end{cases} \tag*{(18)}
Hsr=HrsT.H_{sr}=H_{rs}^{\mathsf{T}}.

Eqs. (16) and (18) then assemble the exact Hessian from scalar terms after the state and costate evaluations, without explicitly differentiating Ψk\Psi_k.

Shifted Warm Start

At sampling instant tt, discard the previously applied input, shift the remaining sequence by one stage, and append the current terminal reference:

Uk∣t0={Uk+1∣t−1⋆,k=0,…,N−2,UN−1∣tr,k=N−1.(19)U_{k|t}^{0}= \begin{cases} U_{k+1|t-1}^{\star},&k=0,\ldots,N-2,\\ U_{N-1|t}^{r},&k=N-1. \end{cases} \tag*{(19)}
Ut0=[(U0∣t0)T⋯(UN−1∣t0)T]T.U_t^0=\left[(U_{0|t}^0)^{\mathsf{T}}\quad\cdots\quad(U_{N-1|t}^0)^{\mathsf{T}}\right]^{\mathsf{T}}.

In the present controller, UN−1∣trU_{N-1|t}^{r} is generated from the terminal reference acceleration by differential flatness. It is used only to complete the initial guess; after optimization, only the first optimized input U0∣t⋆U_{0|t}^{\star} is applied. Since two successive NMPC problems differ only by one sampling interval, the shifted sequence is usually close to the new solution and reduces the number of online iterations.

Overall Algorithm

Algorithm 1 collects the operations at one sampling instant. From the shifted warm start, the state and costate equations give ∇Ji\nabla J_i; the closed-form Hessian expressions give HiH_i; and the nested direction recursion updates the input sequence.

Remark 1 (Extension to Input Constraints): Algorithm 1 solves the unconstrained NMPC problem. If box constraints U‾≤U≤U‾\underline{U}\le U\le\overline{U} are required, the same analytical gradient and Hessian can be combined with a primal active-set method. At iteration ii, components at the lower bound with [∇Ji]r>0[\nabla J_i]_r>0 and those at the upper bound with [∇Ji]r<0[\nabla J_i]_r<0 are placed in the active set; all remaining components form the free set. The active components are fixed, and the nested direction recursion is applied to the gradient and Hessian restricted to the free variables. A feasible step is taken until a new bound is reached, while an active component is released when its gradient no longer satisfies the corresponding sign condition. Thus, constraint handling changes only the active-set identification and the reduced direction calculation without affecting derivative computation through the FBDE recursions.

Algorithm 1 Numerical Algorithm for Solving the NMPC Problem

  1. Input: Measured state X0X_0; reference sequences XrX^r, UrU^r; previous solution Ut−1∗U^*_{t-1}; maximum outer iterations imax⁡i_{\max}; gradient stopping tolerance εg\varepsilon_g; positive-definite regularization matrix R≻0\mathcal{R} \succ0

  2. Output: Final input sequence Ut∗U^*_t and first command U0∣t∗=(Tt,ϕd,t,θd,t)U^*_{0|t} = (T_t, \phi_{d,t}, \theta_{d,t})

  3. Discard the applied U0∣t−1∗U^*_{0|t-1}

  4. Set Uk∣t∗←Uk+1∣t−1∗U^*_{k|t} \leftarrow U^*_{k+1|t-1} for k=0,…,N−2k = 0, \ldots, N - 2

  5. Set UN−1∣t∗←UN−1∣t0U^*_{N-1|t} \leftarrow U^0_{N-1|t} and stack U0=Ut0U_0 = U^0_t according to (19)

  6. Ut∗←U0U^*_t \leftarrow U_0

  7. for i=0i = 0 to imax⁡−1i_{\max} - 1 do

  8. Evaluate ∇Ji=∇UJ(X0,Ui)\nabla J_i = \nabla_U J(X_0, U_i) using (3), (13), and (14)

  9. if ∥∇Ji∥∞<εg\lVert\nabla J_i \rVert_\infty< \varepsilon_g then

  10. Ut∗←UiU^*_t \leftarrow U_i; break

  11. end if

  12. Compute Hi=∇U2J(X0,Ui)H_i = \nabla^2_U J(X_0, U_i) from (16) and (18)

  13. d0(Ui)=(R+Hi)−1∇Jid_0(U_i) = (\mathcal{R} + H_i)^{-1}\nabla J_i

  14. if i≥1i \ge1 then

  15. for j=1j = 1 to ii do

  16. dj(Ui)=(R+Hi)−1[∇Ji+Rdj−1(Ui)]d_j(U_i) = (\mathcal{R} + H_i)^{-1}[\nabla J_i + \mathcal{R}d_{j-1}(U_i)]

  17. end for

  18. end if

  19. Ui+1=Ui−di(Ui)U_{i+1} = U_i - d_i(U_i)

  20. Ut∗←Ui+1U^*_t \leftarrow U_{i+1}

  21. end for

  22. Apply U0∣t∗U^*_{0|t} and retain Ut∗U^*_t for the next sampling instant

Gazebo Validation

Unified Setup and Compared Solvers

The evaluation proceeds from derivative checks to controlled discretization and warm-start ablations, followed by a closed-loop timing comparison at a similar tracking-accuracy level. The Gazebo quadrotor tracks the three-dimensional Lissajous reference xr=3+2sin⁡(ωt)x^r = 3 + 2\sin(\omega t), yr=3+2sin⁡(2ωt)y^r = 3 + 2\sin(2\omega t), and zr=5+1.5sin⁡(3ωt)z^r = 5 + 1.5\sin(3\omega t), where ω=0.15\omega= 0.15. The reference input UrU^r is obtained from the reference acceleration by differential flatness. All controllers operate at 50 Hz with h=0.02h = 0.02 s and N=20N = 20. In both the Gazebo and real-flight tests, Algorithm 1 uses imax⁡=100i_{\max} = 100, εg=10−6\varepsilon_g = 10^{-6}, and R=0.01I3N\mathcal{R} = 0.01I_{3N}.

The comparison includes Algorithm 1, iLQR, acados SQP+HPIPM, and acados SQP+qpOASES. All four solve the same unconstrained NMPC problem and use the same Gazebo plant, reference, prediction horizon, inner-loop controller, thrust-command mapping factor of 0.785, and ROS 2 interfaces. The common cost weights are Q=Qf=diag⁡(10.5,10.5,12,3.8,3.8,5)Q = Q_f = \operatorname{diag}(10.5, 10.5, 12, 3.8, 3.8, 5) and Ru=diag⁡(0.08,8,12)R_u = \operatorname{diag}(0.08, 8, 12). The corresponding NMPC cost is given in (6). Solver-specific numerical settings were tuned separately for good closed-loop performance, while all methods use the same stopping condition ∥∇UJ∥∞<10−6\lVert\nabla_U J\rVert_\infty< 10^{-6}. Each method is evaluated in ten independent closed-loop trials. The reported trajectories and solver-time histories are averaged over the ten trials,

while each tabulated statistic is first computed for an individual trial and then averaged over the ten trials. For all four methods, the solver execution time is measured within the C implementation, from entry to exit of the numerical solver, excluding reference generation, state estimation, ROS 2 communication, and command publication. This measurement is hereafter referred to as the C-core solution time.

A central-difference check gives relative errors of 5.286×10−95.286 \times10^{-9} and 1.440×10−101.440 \times10^{-10} for the gradient and complete Hessian, respectively. The Hessian is symmetric to the reported numerical precision, confirming the FBDE recursions and the closed-form weights in (17).

Discretization and Warm-Start Ablations

For the ablations, the same 2828 post-transient states and references are replayed with different discretizations and initializations. This removes run-to-run plant variation from the comparison. The replay is offline; its times are not treated as closed-loop solver measurements.

As shown in Figure 3(a), the one-step position error of forward Euler scales as O(h2)O(h^2) with the step size hh. The RK4 error remains at floating-point level because the held-input dynamics are integrated exactly by the RK4 model. Figure 3(b) shows that the shifted warm start reduces the mean iteration count from 6.738 to 4.925 and the mean replay time from 0.2202 to 0.08656 ms. These changes correspond to reductions of 26.91% and 60.70%, respectively. Each solve is repeated five times with alternating call order, and reference generation is excluded from the timing. The absolute replay times in Figure 3 are therefore not compared directly with Table I.

Figure 3. Controlled same-state replay ablations using states recorded from the Gazebo closed-loop experiment for Algorithm 1: (a) one-step position error of the RK4 and forward-Euler models; (b) mean iteration count and offline-replay solver time with cold and shifted warm starts.

Solver-Time and Iteration Comparison

All four methods complete the three-dimensional trajectory in Figure 4. Their 3-D RMSE values differ by at most 4.857 mm, providing a comparable tracking level for the solver-time comparison. With this tracking comparison established, Figure 5 shows how the C-core solution times vary during closed-loop operation. Algorithm 1 remains near 0.12 ms for most of the evaluated interval, below the other three methods, while iLQR varies more widely. The unsmoothed statistics are reported in Table I.

Gazebo three-dimensional trajectory tracking for the four methods

Figure 4. Gazebo three-dimensional trajectory tracking for the four methods.

Gazebo C-core solver-time histories for the four methods

Figure 5. Gazebo C-core solver-time histories for the four methods.

Algorithm 1 requires 4.92 iterations on average and never more than five. Its mean, 99th percentile (P99), and maximum solve times are 0.1197, 0.2771, and 0.6049 ms, respectively. Relative to iLQR, acados SQP+HPIPM, and acados SQP+qpOASES, its mean time is lower by 52.82%, 56.44%, and 49.24%, or factors of 2.12, 2.30, and 1.97. Iteration counts have different meanings across the solvers, but Algorithm 1 meets its stopping condition within five iterations at every sampling instant.

The timing difference is consistent with the solver design: analytical derivatives avoid repeated numerical differentiation, the OCP refinements reuse one inverse per outer iteration, and the shifted previous solution initializes the next solve. No QP subproblem or general-purpose nonlinear programming package is used inside the online solve. Whether this advantage persists on the onboard processor is examined next.

Real-Flight Validation

Flight Platform and Comparison Setup

The real-flight tests examine whether the Gazebo timing advantage persists on the onboard processor. The four methods in Table II—Algorithm 1, iLQR, acados SQP+HPIPM, and acados SQP+qpOASES—are tested onboard in ten independent flights each. All four solve the same unconstrained NMPC problem. Each controller tracks a circle of 1.0 m radius at an

MethodMeanP95P99Max.Iter. (mean/max.)
Algorithm 10.31210.43161.31053.77544.5/5
iLQR1.05233.06145.587213.13944.7/11
acados SQP+HPIPM1.80144.50298.650614.99862.4/5
acados SQP+qpOASES1.73273.98707.555117.41782.4/5

Table II. Measured real-flight performance (C-core times in ms)

MethodMeanP99Max.Iterations mean/max.
Algorithm 10.11970.27710.60494.92/5
iLQR0.25370.66590.94056.43/10
acados SQP+HPIPM0.27480.37000.69402.66/3
acados SQP+qpOASES0.23580.35400.56802.66/3

Table 1. Gazebo performance (C-core times in ms)

altitude of approximately 0.5 m. The trajectory and controller parameters keep the commanded thrust and attitude within the platform’s safe operating range. Each flight covers three 20-s laps, with controller, reference, and state-estimation data recorded at approximately 50 Hz. Reported trajectories and timing statistics are averaged across the ten flights.

The experimental platform is shown in Figure 6. It is built on a 3.5-inch frame, with a MicoAir743v2-AIO-35A board providing flight control and motor drive. All optimization methods run on the same NVIDIA Jetson Orin NX 8 GB computer through identical ROS 2 interfaces. LiDAR and inertial measurements are processed by FAST-LIO to provide odometry, which is fused with onboard IMU measurements by the EKF for state feedback. The airframe, sensors, computer, communication interfaces, and inner-loop controller remain unchanged across the runs; only the NMPC solution method is replaced.

Experimental quadrotor platform

Figure 6. Experimental quadrotor platform.

Solver-Time Comparison

In Figure 7, all four controllers complete the circular task in closed loop. Their 3-D tracking RMSE values are at most 0.1174 m; the comparison below focuses on solver time within this tracking-error range.

Measured three-dimensional trajectory tracking for the four methods

Figure 7. Measured three-dimensional trajectory tracking for the four methods.

Figure 8 shows the measured C-core solve times over the flight. Algorithm 1 is below the three baselines for most of the interval; the difference is not confined to a few samples. Table II reports the unsmoothed statistics and iteration counts.

Measured real-flight solver-time histories for the four methods

Figure 8. Measured real-flight solver-time histories for the four methods.

Algorithm 1 takes 4.5 iterations on average and at most five. Its mean, 95th percentile (P95), P99, and maximum solve times of 0.3121, 0.4316, 1.3105, and 3.7754 ms are the smallest in Table II. Mean-time speedup factors relative to iLQR, acados SQP+HPIPM, and acados SQP+qpOASES are 3.37, 5.77, and 5.55, respectively. The P99 times are lower by 76.54%–84.85%. The two acados configurations take fewer iterations on average, yet require more time per completed NMPC solve.

The maximum time of each method remains below the 20 ms sampling period. In particular, the 3.7754 ms worst-case time of Algorithm 1 occupies only 18.9% of this period and corresponds to a solver-only rate of approximately 264.9 Hz. This value is not an end-to-end closed-loop rate because state estimation, communication, and command publication also require time. The measured mean and worst-case results leave substantial computational margin for high-frequency onboard implementation.

The mean-time ordering is the same in Gazebo and flight: Algorithm 1 is fastest, by factors of 1.97–2.30 and 3.37–5.77, respectively. Different trajectories and operating environments preclude a direct comparison of absolute times, but the advantage over the baselines persists onboard.

Conclusion

This work addressed the online solution cost of UAV NMPC while retaining the nonlinear translational predictor. The structure of that model permits an exact discrete-time RK4 update and compact FBDE recursions for the gradient and exact Hessian of the finite-horizon cost with respect to the input sequence. These derivatives feed the OCP nested direction recursion, with a shifted warm start carrying the previous solution into each new sampling instant. In Gazebo, the solver achieved lower C-core time than the three baselines at similar

tracking accuracy. The same timing advantage was observed onboard: the mean solution-time speedups ranged from 3.37 to 5.77, and the longest measured solve remained below the 20 ms control period. Future work will address input constraints and measure the complete estimation–optimization–actuation latency at higher closed-loop rates.

References

  1. [1]D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. IEEE Int. Conf. Robot. Autom., 2011, pp. 2520–2525.DOI
  2. [2]D. Q. Mayne, “Model predictive control: Recent developments and future promise,” Automatica, vol. 50, no. 12, pp. 2967–2986, 2014.DOI
  3. [3]H. Liang, H. Li, and D. Xu, “Nonlinear model predictive trajectory tracking control of underactuated marine vehicles: Theory and experiment,” IEEE Trans. Ind. Electron., vol. 68, no. 5, pp. 4238–4248, May 2021.
  4. [4]S. Sun, A. Romero, P. Foehn, E. Kaufmann, and D. Scaramuzza, “A comparative study of nonlinear MPC and differential-flatness-based control for quadrotor agile flight,” IEEE Trans. Robot., vol. 38, no. 6, pp. 3357–3373, Dec. 2022.arxiv.org/abs/2109.01365
  5. [5]L. F. Recalde, D. Agrawal, J. Arrizabalaga, and G. Li, “DQ-NMPC: Dual-quaternion NMPC for quadrotor flight,” IEEE Robot. Autom. Lett., vol. 10, no. 12, pp. 12 644–12 651, Dec. 2025.DOI
  6. [6]C. Gu, Q. Wu, Y.-W. Wang, and W.-A. Zhang, “Prescribed performance disturbance rejection tracking control of UAVs via high-order fully actuated system approach,” IEEE Trans. Aerosp. Electron. Syst., vol. 62, pp. 714–726, 2026.
  7. [7]A. R. P. Andrién, E. Lefeber, D. Antunes, and W. P. M. H. Heemels, “Model predictive control for quadcopters with almost global trajectory tracking guarantees,” IEEE Trans. Autom. Control, vol. 69, no. 8, pp. 5216–5230, Aug. 2024.DOI
  8. [8]M. Sarvaiya, G. Li, and G. Loianno, “HPA-MPC: Hybrid perception-aware nonlinear model predictive control for quadrotors with suspended loads,” IEEE Robot. Autom. Lett., vol. 10, no. 1, pp. 358–365, Jan. 2025.arxiv.org/abs/2411.11982
  9. [9]T. Çimen and S. P. Banks, “Nonlinear optimal tracking control with application to super-tankers for autopilot design,” Automatica, vol. 40, no. 11, pp. 1845–1863, 2004.DOI
  10. [10]D. Wang, Q. Pan, Y. Shi, J. Hu, and C. Zhao, “Efficient nonlinear model predictive control for quadrotor trajectory tracking: Algorithms and experiment,” IEEE Trans. Cybern., vol. 51, no. 10, pp. 5057–5068, Oct. 2021.DOI
  11. [11]M. A. K. Gomaa, O. D. Silva, G. K. I. Mann, and R. G. Gosine, “Computationally efficient stability-based nonlinear model predictive control design for quadrotor aerial vehicles,” IEEE Trans. Control Syst. Technol., vol. 31, no. 2, pp. 615–630, Mar. 2023.DOI
  12. [12]S. Omidi, “SDC-based model predictive control: Enhancing computational feasibility for safety-critical quadrotor control,” arXiv:2509.24208, 2025.arxiv.org/abs/2509.24208
  13. [13]V. C. Nguyen, P. Walunj, C. Le, A. D. Nguyen, and H. M. La, “NMPCM: Nonlinear model predictive control on resource-constrained microcontrollers,” arXiv:2507.21259, 2025.arxiv.org/abs/2507.21259
  14. [14]S. M. Rajkumar, C. Yang, Y. Gu, S. Cheng, N. Hovakimyan, and D. Goswami, “Real-time linear MPC for quadrotors on SE(3): An analytical Koopman-based realization,” IEEE Robot. Autom. Lett., vol. 10, no. 12, pp. 13 018–13 025, Dec. 2025.arxiv.org/abs/2409.12374
  15. [15]Z. Bao, K. Cai, Q. Wang, X. Liu, and P. Zhao, “SER-MPC: Singularity-free robust event-triggered MPC for UAVs subject to external disturbances,” IEEE Trans. Ind. Electron., 2026, early access, doi: 10.1109/TIE.2026.3697458.DOI
  16. [16]J. Nocedal and S. J. Wright, Numerical Optimization. New York, NY, USA: Springer, 1999.
  17. [17]J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi: A software framework for nonlinear optimization and optimal control,” Math. Program. Comput., vol. 11, no. 1, pp. 1–36, 2019.DOI
  18. [18]A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Math. Program., vol. 106, no. 1, pp. 25–57, 2006.DOI
  19. [19]B. Lindqvist, S. A. Mansouri, A. Agha-mohammadi, and G. Nikolakopoulos, “Nonlinear MPC for collision avoidance and control of UAVs with dynamic obstacles,” IEEE Robot. Autom. Lett., vol. 5, no. 4, pp. 6001–6008, Oct. 2020.DOI
  20. [20]F. L. Chernousko and A. Lyubushin, “Method of successive approximations for solution of optimal control problems,” Optimal Control Appl. Methods, vol. 3, no. 2, pp. 101–114, 1982.DOI
  21. [21]H. Zhang, H. Wang, Y. Xu, and Z. Guo, “Optimization methods rooted in optimal control,” Sci. China Inf. Sci., vol. 67, no. 12, pp. 1–9, 2024.DOI
  22. [22]C. Lv, H. Li, H. Zhang, and M. Fu, “Nonlinear optimal control based on FBDEs and its application to AGV,” IEEE Trans. Cybern., vol. 54, no. 11, pp. 6448–6457, Nov. 2024.DOI
  23. [23]C. Lv, H. Li, K. Peng, X. Yin, and H. Zhang, “Nonlinear optimal control with control constraints based on FBDEs and its application to AGV,” IEEE Trans. Ind. Electron., vol. 72, no. 6, pp. 6402–6411, Jun. 2025.DOI
  24. [24]C. Lv, X. Yin, H. Li, and H. Zhang, “Optimal control of nonlinear systems using accelerated gradient and Hessian computation,” IEEE Trans. Ind. Electron., vol. 73, no. 3, pp. 4645–4653, Mar. 2026.

Paper details

vlnc:W_qzgz4ceb
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY 4.0
Usage

views · 0

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Mathematics

Not recorded for this paper

View statement
Looking for related papers…
Contents