Nonlinear Control of Quadcopters via Approximate Dynamic Programming

We consider the discrete-time dynamical system of a quadcopter with continuous state and input spaces and aim to minimize a discounted cost objective. The specific states, inputs and dynamics of the quadcopter will be given in Section III. For now, let us denote the state of the system at time $k$ as $\smash{x_{k}\in\mathbb{R}^{n_{x}}}$, and the control input as $\smash{u_{k}\in\mathbb{R}^{n_{u}}}$. The evolution of the system is described by the dynamics function $\smash{f:\mathcal{X}\times\mathcal{U}\to\mathcal{X}}$ such that

$$x_{k+1}=f(x_{k},u_{k}).$$

(1)

The stage cost function is the non-negative cost of taking decision $u_{k}$ when being in state $x_{k}$, denoted as $\smash{l:\mathcal{X}\times\mathcal{U}\rightarrow\mathbb{R}_{+}}$. Given a discount factor $\smash{\xi\in[0,1)}$ and a stationary control policy of the form $\smash{\pi:\mathcal{X}\rightarrow\mathcal{U}}$, the discounted infinite horizon cost is thus,

$$J_{\pi}(x)=\sum_{k=0}^{\infty}\xi^{k}\,l(x_{k},\pi(x_{k})),\hskip 28.45274pt\text{with $x_{0}=x$.}$$

(2)

The aim is to find the policy that minimizes (2) for all $x\in\mathcal{X}$. The solution is characterized by the optimal value function $\smash{V^{*}:\mathcal{X}\to\mathbb{R}}$ that satisfies the Bellman equation,

$$V^{*}(x)\,=\,\underbrace{\,\min_{u\in\mathcal{U}}\hskip 2.84544ptl(x,u)+\xi\,V^{*}(f(x,u))\,}_{\mathcal{T}V^{*}}\,,\hskip 14.22636pt\forall x\in\mathcal{X},$$

(3)

where $\mathcal{T}$ is the Bellman operator. Given a solution of (3) the optimal policy is the so called greedy policy,

$$\pi^{*}(x)=\operatorname*{arg\,min}_{u\in\mathcal{U}}\hskip 2.84544ptl(x,u)+\xi\,V^{*}(f(x,u)).$$

(4)

which minimizes the cost of taking the decision $u$ now, plus the cost-to-go from the next time period onwards. We require the standard assumptions on the stage cost, dynamics, and existence of feasible policies to ensure that $V^{*}$ and $\pi^{*}$ exist, are time-invariant, and attain the minimum, for example [17, Assumption 4.2.1, 4.2.2] or [18, §1.2]. For a general problem instance, solving (3) and implementing (4) is intractable.

The monotone and contractive properties of the Bellman operator mean that any function $\smash{\hat{V}:\mathcal{X}\to\mathbb{R}}$ satisfying the so called Bellman inequality is a point-wise under-estimator of $V^{*}$, i.e.,

$$\hat{V}(x)\leq\mathcal{T}\hat{V}(x),\,\forall x\!\in\!\mathcal{X}\hskip 2.84544pt\Rightarrow\hskip 2.84544pt\hat{V}(x)\leq V^{*}(x),\,\forall x\!\in\!\mathcal{X},$$

(5)

where $\hat{V}$ is referred to as an approximate value function. This is the key property that motivates the various LP approaches to ADP proposed in [5, 8, 10, 11, 13]. Letting $\smash{\mathcal{F(X)}}$ denote a space of functions mapping $\smash{\mathcal{X}\rightarrow\mathbb{R}}$, and using the Bellman inequality, a point-wise under-estimator is found by solving the following optimization problem,

	$\displaystyle\underset{\hat{V}(x)}{\text{maximize}}\hskip 5.69046pt$	$\displaystyle{\displaystyle\int\hat{V}(x)\,c(x)\,dx}$		(6a)
	subject to	$\displaystyle\;\;\hat{V}\in\mathcal{F(X)}$		(6b)
		$\displaystyle\;\;\hat{V}(x)\leq\mathcal{T}\hat{V}(x),\hskip 2.84544pt\forall x\in\mathcal{X},$		(6c)

where the state relevance weighting function, $c(x)$, is a finite measure on the state space and it allows the practitioner to select the region where a better approximation is desired. When $\mathcal{F(X)}$ is the space of real-valued measurable functions on $\mathcal{X}$ it is proven in [17] that the solution to (6) solves the Bellman equation for $c$-almost all $\smash{x\in\mathcal{X}}$, however the infinite dimensional decision variable makes the problem intractable.

In this paper we use the space of polynomials for the function space $\mathcal{F(X)}$. This choice is motivated by [10] where the authors show that problem (6) can be reformulated as a finite SDP when the dynamics, stage costs, and spaces are described by polynomials. Letting $\smash{\mathcal{P}_{d}\mathcal{(X)}}$ denote the space of polynomials up to degree $d$, we replace (6b) by $\smash{\hat{V}\in\mathcal{P}_{d}\mathcal{(X)}}$ and hence the decision variables are the coefficients of the monomials up to degree $d$. Considering the minimization in the Bellman operator $\mathcal{T}$, the infinite constraints (6c) can be equivalently written as,

$$0\leq\underbrace{-\hat{V}(x)+l(x,u)+\xi{\hat{V}(f(x,u))}\,}_{b(x,u)},\hskip 2.84544pt\forall\,x\!\in\!\mathcal{X},\,u\!\in\!\mathcal{U},$$

(7)

where $b(x,u)$ is a polynomial when $l$ and $f$ are polynomials.

We now use the SOS S-Procedure [19] to reformulate (7) as a single Linear Matrix Inequality (LMI) constraint on the decision variable $\smash{\hat{V}\in\mathcal{P}_{d}\mathcal{(X)}}$. Letting $SOS$ denote the set of polynomials that are representable as a sum of polynomials squared, then from [19, Theorem 3.3] we have the following equivalence for certifying that a polynomial $\smash{v\in\mathcal{P}_{2d}\mathcal{(X)}}$ is an SOS polynomial,

$$v(x)\in SOS\hskip 4.26773pt\Leftrightarrow\hskip 4.26773pt\exists M\succeq 0,\hskip 2.84544pt\text{s.t.}\hskip 2.84544ptv(x)=z(x)^{T}Mz(x),$$

(8)

where $z(x)$ is the vector of monomials of $\smash{x\in\mathbb{R}^{n_{x}}}$ up to degree $d$, and $M$ is a square matrix of size ${n_{x}+d\choose d}$. Letting $\smash{g_{i}(x,u)\geq 0}$ denote the polynomials that describe the state and input spaces ($\mathcal{X}$ and $\mathcal{U}$), then the following set of equations,

	$\displaystyle b(x,u)-\lambda(x,u)\sum\nolimits_{i}g_{i}(x,u)$	$\displaystyle\,\in\,SOS,$		(9a)
	$\displaystyle\lambda(x,u)$	$\displaystyle\,\in\,SOS,$		(9b)

is a sufficient reformulation of (7) in the sense that ${\text{\eqref{eq:SOS_procedure}}\Rightarrow\text{\eqref{eq:BI_relaxed}}}$. Replacing (6c) with (9), the multiplier $\smash{\lambda\in\mathcal{P}_{d}(\mathcal{X}\!\times\!\mathcal{U})}$ is an additional decision variable with the polynomial degree of the multiplier as a choice. It is reasonable to choose the largest multiplier degree for which the LMI constraints stemming from (9) are computationally tractable.

Finally, the objective (6a) is linear in the coefficients of $\smash{\hat{V}\in\mathcal{P}_{d}\mathcal{(X)}}$ and requires knowledge of the moments of the state relevance weighting function $c(x)$ up to order $d$. Thus, a point-wise under-estimator of $V^{*}$ can be found using a commercial solver for the SDP relaxation of (6).

To improve the quality of the approximate value function, we use the approach proposed in [11] that solves a sequence of optimization problems, each with constraints of the same size as (9).

First, we solve the SDP relaxation of (6) for a particular choice of the state relevance weighting, and denote the solution as $\hat{V}_{1}^{*}$. Then we solve the SDP relaxation of the following optimization problem with $j=2$,

	$\displaystyle\underset{\hat{V}_{j}}{\text{maximize}}\hskip 5.69046pt$	$\displaystyle\displaystyle\int\hat{V}_{j}(x)\,c(x)\,dx$		(10a)
	subject to	$\displaystyle\hat{V}_{j}\in\mathcal{P}_{d}\mathcal{(X)},$		(10b)
		$\displaystyle\hat{V}_{j}(x)\leq\left(\mathcal{T}\underset{k=1,\dots,j-1}{\text{max}}\hat{V}^{*}_{k}\right)(x),\hskip 5.69046pt\forall x\!\in\!\mathcal{X}$		(10c)

where $\hat{V}_{1}^{*}$ is fixed problem data, and we denote the solution as $\hat{V}_{2}^{*}$. We then solve the SDP relaxation of (10) iteratively for ${j\geq 3}$, each time storing the solution $\hat{V}_{j}^{*}$ as fixed data to be used in the next iteration. As the cost function (10a) is non-decreasing with the iterations of (10), we terminate when the improvement in the cost function becomes less than a pre-specified threshold. The approximate value function for a particular choice of $c(x)$ is set as the solution of the final iteration.

The steps given in [11] show how to reformulate (10c) as a polynomial inequality constraint similar to (7). The SOS S-Procedure is then applied and the resulting relaxation involves one LMI constraint with the same size as (9a), and $\smash{j-1}$ LMI constraints identical to (9b).

To construct an online policy, we first construct our best approximation to the value function, and we use this as a surrogate for $V^{*}$ in the greedy policy (4). For different choices of the state relevance weighting, denoted $c_{i}(x)$ for $\smash{i=1,\dots,N_{c}}$, we perform the iteration described in Section II-C and denote $\hat{V}_{c_{i}}^{*}$ as the value function estimate returned by the final iteration. We take the PWM of these under-estimators as our best estimate for $V^{*}$, i.e.,

$$\max_{i=1,\dots,N_{c}}\hskip 2.84544pt\hat{V}_{c_{i}}^{*}(x)\hskip 5.69046pt\leq\hskip 5.69046ptV^{*}(x),\hskip 5.69046pt\forall x\in\mathcal{X}.$$

(11)

The online policy, also called the approximate greedy policy, is thus,

$\displaystyle\hat{\pi}(x)$

$\displaystyle=\operatorname*{arg\,min}_{u\in\mathcal{U}}\hskip 2.84544ptl(x,u)+\xi\,\max_{i=1,\dots,N_{c}}\hskip 2.84544pt\hat{V}_{c_{i}}^{*}(f(x,u)),$

(12)

where the maximum over the $\hat{V}_{c_{i}}^{*}$ is readily reformulated using an epigraph variable.

To construct the surrogate for $V^{*}$, it remains to choose the $c_{i}$ weightings and in Section IV we provide the weightings used for nonlinear control of a quadcopter. The surrogate for $V^{*}$ can be considered as a sufficiently accurate estimate if the online performance of (12) achieves a low cost.

To describe the dynamics of the quadcopter, two frames of reference are used, the inertial (world) frame and the body frame (attached to the quadcopter). The location of the body frame with respect to the inertial frame is denoted as ${\vec{p}=[p_{x},p_{y},p_{z}]^{\top}}$ and shown in Figure 1. The angular rates about the body frame axes $x^{(B)}$, $y^{(B)}$ and $z^{(B)}$ are denoted ${\vec{\omega}=[w_{x},w_{y},w_{z}]^{\top}}$. The orientation of the body frame relative to the inertial frame is given by ${\vec{\psi}=[\gamma,\beta,\alpha]^{\top}}$, the roll, pitch, and yaw intrinsic Euler angles, respectively. Using the ZYX convention for the Euler angles, and with ${s_{(\cdot)}:=\sin(\cdot)}$, ${c_{(\cdot)}:=\cos(\cdot)}$, the equations of motion for a quadcopter of mass $m$ and with moment of inertia $J$, are readily derived as,

	$\displaystyle\ddot{\vec{p}}^{(I)}$	$\displaystyle=\frac{1}{m}\sum\limits_{i=1}^{4}f_{i}\begin{bmatrix}c_{\alpha}s_{\beta}s_{\gamma}+s_{\alpha}c_{\gamma}\\ s_{\alpha}s_{\beta}c_{\gamma}-c_{\alpha}s_{\gamma}\\ c_{\beta}c_{\gamma}\end{bmatrix}-\begin{bmatrix}0\\ 0\\ g\end{bmatrix},$		(13a)
	$\displaystyle\begin{bmatrix}\dot{\omega_{x}}\\ \dot{\omega_{y}}\\ \dot{\omega_{z}}\end{bmatrix}$	$\displaystyle=J^{-1}\left(\begin{bmatrix}\sum_{i=1}^{4}f_{i}d_{y_{i}}\\ \sum_{i=1}^{4}-f_{i}d_{x_{i}}\\ \sum_{i=1}^{4}f_{i}d_{\tau_{i}}\end{bmatrix}-\vec{\omega}\times J\vec{\omega}\right),$		(13b)

where $f_{i}$ is the thrust produced by propeller $i$, $d_{y_{i}}$ and $d_{x_{i}}$ are the distances from the axis of propeller $i$ to the center of gravity of the quadcopter, $d_{\tau_{i}}$ is a constant of proportionality for how much torque is produced by thrust $f_{i}$, and $g$ is the acceleration due to gravity. For more details on the derivation of the dynamics refer to [20]. The system thus has 12 states, given by $[\vec{p}^{\,\top},\dot{\vec{p}}^{\,\top},\vec{\psi}^{\,\top},\vec{\omega}^{\,\top}]^{\top}$, and four inputs, given by the motor thrusts $f_{i}$, $i=1,...,4$.

In order to apply the ADP method from Section II, we approximate the full dynamics of (13) in three steps. First, we exploit time scale separation to reduce the state dimension of the dynamics by using the cascaded control structure shown in Figure 2. The inner controller considers the states $[\vec{\psi\,}^{\top},\vec{\omega}^{\top}]^{\top}$ and uses the thrust inputs $f_{1},\dots,f_{4}$ to track a reference attitude $\vec{\psi}_{\mathrm{ref}}$ and total thrust $f_{T,\mathrm{ref}}$. The outer controller considers the states ${x=[\,\vec{p}^{\top},\dot{\vec{p}}^{\top}\,]^{\top}}$ and uses the inputs ${u=[\gamma,\beta,\alpha,f_{T}]^{\top}}$ to stabilize the quadcopter to a specified position and yaw setpoint. These inputs are given as references to the inner controller. This cascaded structure is common practice for quadcopter control [15], and the approximation step assumes a sufficient time-scale separation so that the transients of the inner loop controller can be neglected when designing the outer loop controller. The goal is to design the outer controller using the combination of ADP methods described in Section II.

In the second approximation step, we use a Taylor expansion of (13a) to replace the trigonometric functions with a polynomial approximation that fits the SOS framework described in Section II-B. We use a third order Taylor expansion as a compromise between the accuracy of the approximated nonlinear dynamics and the size of the optimization problem (10) for fitting approximate value functions. The translational dynamics (13a) are thus approximated as

$$\begin{bmatrix}\dot{\vec{p}}\\ \frac{1}{m}(\beta f_{T}-\frac{1}{2}mg\alpha^{2}\beta+f_{T}\alpha\gamma-\frac{1}{6}mg\beta^{3}-\frac{1}{2}mg\beta\gamma^{2})\\ \frac{1}{m}(-\gamma f_{T}+f_{T}\alpha\beta+\frac{1}{2}mg\alpha^{2}\gamma+\frac{1}{6}mg\gamma^{3})\\ \frac{1}{m}((f_{T}-mg)-\frac{1}{2}f_{T}\beta^{2}-\frac{1}{2}f_{T}\gamma^{2})\\ \end{bmatrix},$$

(14)

where we have introduced ${f_{T}=\sum_{i}f_{i}}$ to represent the total thrust.

Finally, to bring the dynamics of (14) to discrete time, we use a forward Euler scheme. The discrete-time dynamics are required for applying the ADP methods to the design of the outer-loop controller. We choose a forward Euler approximation in order not to increase the polynomial order of the dynamics, and assume that the sampling time is fast enough to maintain stability properties of the plant. For the stage cost we chose the quadratic form, ${l(x,u)=x^{\top}Qx+u^{\top}Ru}$.

We chose to constrain the tilt angle that the $z^{(I)}$ axis forms with the $z^{(B)}$ axis, denoted by $\theta$ and shown in Figure 3. The motivation behind this choice is linked to the nonlinearities in the dynamics. We choose $\theta$ large enough so that the nonlinearities are relevant for the solution of the optimal control problem, but small enough so that the third order Taylor expansion (14) is an adequate approximation of the true dynamics (13a). The angle constraint is given by

$$\cos(\theta)\leq\cos(\beta)\cos(\gamma),$$

(15)

which is nonlinear in the inputs $\gamma$ and $\beta$, similar to the constraint introduced in [15].

To represent the physical actuator limits of the propellers, we impose a lower and upper bound on the total thrust, $f_{T}$. Denoting the lower and upper bound as $f_{T,lb}$ and $f_{T,ub}$, respectively, the constraint is thus,

$$f_{T,lb}\leq f_{T}\leq f_{T,ub}.$$

(16)

In addition to the dynamics, the constraints also need to be in polynomial form in order to apply the SOS techniques from Section II-B. We chose quadratic forms for the constraints in (15) and (16). The constraint on the angle in (15) is approximated via least-squares to the closest ellipse with radii $a_{1}$ and $a_{2}$, and the constraint on the total thrust in (16) can be transformed into a single second order polynomial to reduce the number of SOS multipliers. The constraint functions are thus given by

	$\displaystyle 1-\frac{\gamma^{2}}{{a_{1}}^{2}}-\frac{\beta^{2}}{{a_{2}}^{2}}=:g_{\theta}(u)$	$\displaystyle\geq 0,$		(17a)
	$\displaystyle f_{T}\left(f_{T,lb}+f_{T,ub}\right)-f_{T,lb}f_{T,ub}-f_{T}^{2}=:g_{f}(u)$	$\displaystyle\geq 0.$		(17b)

Note that $g_{f}(u)$ and $g_{\theta}(u)$ correspond to $g_{i}(x,u)$ in (9).

Heuristics have been developed for how to choose the state relevance weighting, $c(x)$, which is the main tuning parameter. As presented in [12], to alleviate the sensitivity of this choice, we select a family of $\smash{c_{i}(x)\sim\mathcal{N}(0,\Sigma_{i})}$ as Gaussians with zero mean and different covariance matrices. Let $\mathrm{diag}(v)$ define a diagonal matrix with the entries of vector $v$ on its diagonal. Then, with ${\Sigma_{0}=\mathrm{diag}([0.1,0.1,0.1,1,1,1])}$, the set of covariance matrices $\Sigma_{i}$ used is obtained as,

		$\displaystyle\Sigma_{i}=K_{i}\,\Sigma_{0},~i=\{1,.,9\},~\text{with}$		(18)
		$\displaystyle K_{i}\in\{01,01,03,05,07,1,13,15,2\}.$

For each different $c_{i}(x)$ in (18), we perform the iterative procedure described in Section II-C. As shown in Figure 4, the relative cost improvement converges to under a threshold of $10^{-5}$ within 9 iterations.

In Figure 5 (a), a cut of the value function approximation for different iterations for the same $c_{i}(x)$ is shown. The cut is obtained by setting all states to zero except $\dot{p}_{z}$. Solving this iterative problem for the different $c_{i}(x)$ results in a family of value function approximations, $\hat{V}_{c_{i}}^{*}(x)$, $i=1,...,N_{c}$. As presented in Section II-D, the point-wise maximum (PWM) of this family, ${\hat{V}^{*}_{PWM}(x):=\underset{i=1,\dots,N_{c}}{\text{max}}\hat{V}^{*}_{c_{i}}(x)}$, is the value function approximation that is used for the online greedy policy (12). Figure 5 (b) shows a cut in the $\dot{p}_{z}$ state of the value function approximations $\hat{V}_{c_{i}}^{*}(x)$ and of $\hat{V}^{*}_{PWM}(x)$.

We consider the model of a small-sized quadcopter, the Crazyflie 2.0, [21], with a mass of 27 $g$. Details about the model parameters such as its inertia and dimensions can be found in [22], [23]. For the simulation the full state dynamics of the quadcopter as in (13) are used, together with the control structure shown in Figure 2. The inner loop tracks the commanded reference Euler angles $\psi_{ref}$ with a PID controller running at 500 Hz, with the controller parameters and implementation details found in [23]. The outer control loop, and hence all the controllers listed in Table I, runs at 50 Hz.

In the ADP approach, the matrices for the stage cost, ${l(x,u)=x^{\top}Qx+u^{\top}Ru}$, have been chosen as,

	$\displaystyle Q$	$\displaystyle=\mathrm{diag}(\begin{bmatrix}50&50&50&5&5&5\end{bmatrix}),$		(19)
	$\displaystyle R$	$\displaystyle=\mathrm{diag}(\begin{bmatrix}20&20&100&1500\end{bmatrix}),$

and the discount factor is set to $\xi=0.98$. The total thrust is constrained to ${f_{T}\!\in\![0N,\,0.56N]}$. For the angle constraint in (15) we have chosen $\theta=45^{\circ}$, which is large enough to demonstrate that the controller takes into account the nonlinearities of the system, but small enough for the constraint to become active.

At each time step, the online policy in (12) is solved and the optimal input is applied to the system. As the term $\hat{V}_{c_{i}}^{*}(f(x,u))$ is a composition of a quadratic and a third order polynomial, the resulting optimization problem is not convex. We refer to this as NL-GP-ADP, see Table I, and we solve it in simulation with an interior point method.

In the following, we compare NL-GP-ADP to a linear time-varying (LTV) MPC, [24], which also accounts for the nonlinear dynamics in its predictions. At each time step, the dynamics (13a) are linearized around the predicted trajectory, $x_{\mathrm{traj},k}$, $u_{\mathrm{traj},k}$, and denoted by $A_{k},B_{k},g_{k}$. Then, the following problem is solved,

		$\displaystyle\underset{\{u_{k}\}_{k=0}^{N-1}}{\text{minimize}}$		$\displaystyle\sum_{k=0}^{N-1}\lVert x_{k}-x_{\mathrm{traj},k}\rVert_{Q}^{2}+\lVert u_{k}-u_{\mathrm{traj},k}\rVert_{R}^{2}$		(20)
		$\displaystyle\hskip 78.24507pt+\lVert x_{N}-x_{\mathrm{traj},N}\rVert_{P}^{2}$
		subject to		$\displaystyle x_{k+1}=A_{k}x_{k}+B_{k}u_{k}+g_{k},$
		$\displaystyle u_{k}\in\mathcal{U},$

where the set $\mathcal{U}$ is defined as in (17), making this problem a convex Quadratically Constrained Quadratic Program (QCQP). Note that the $x_{k}$ and $u_{k}$ variables are deviations from the predicted trajectory. The cost matrices $Q$ and $R$ are the same as in (19), and $P$ comes from the solution to the Riccati equation using the linearization of dynamics (13a) around the hover state ${[\,\vec{p},\dot{\vec{p}}\,]^{\top}}\!=\!0$. The first input, $u_{0}^{*}$, is applied, and the system evolves by one time step. The predicted trajectory for the next iteration, i.e., $x_{\mathrm{traj},k}$, $u_{\mathrm{traj},k}$, is based on the state measurement and the inputs $u_{1}^{*},...,u_{N-1}^{*}$. We refer to this as LTV-MPC-LQR of horizon length $N$, see Table I.

Furthermore, a comparison of the NL-GP-ADP policy to a controller that does not account for the nonlinear dynamics is given. For this we use a linear time-invariant (LTI) MPC, i.e., for the dynamics we use the linearized model around hover and the same $Q$, $R$, and $P$ cost and Riccati solution matrices as in (20). Consistent with the naming convention, this policy is referred to as LTI-MPC-LQR of horizon $N=1$, and the policy is also a convex QCQP. This policy is similar to the ADP approach demonstrated on miniature helicopters in [10], and for the control task we consider the performance was almost identical. Thus, for the sake of clarity, we do not include a policy based on [10] in the comparison here.

A comparison between the three policies is shown in Figure 6. The main difference between the approaches is the drop in $p_{z}$ due to the fact that NL-GP-ADP and LTV-MPC-LQR have more information about the nonlinearities of the system than LTI-MPC-LQR which uses the linearized dynamics around hover. The nonlinearity of interest for this test is the change in vertical thrust when the quadcopter tilts, and as seen in Figure 6 (b), LTI-MPC-LQR corrects for the drop in $p_{z}$ after it happens. However, NL-GP-ADP and LTV-MPC-LQR pre-compensate for this drop by increasing the thrust in advance.

For the experiments, the Crazyflie quadcopter is used together with a set of infrared cameras that provide the system with position and attitude measurements, [25]. Body frame angular rate measurements come from the on-board IMU. The control loops are the same as described in Section IV-A for the simulations.

Implementation of the NL-GP-ADP in real-time (i.e., at 50 Hz) was computationally intractable. To overcome this difficulty, the ADP approach is combined with the linear time-varying MPC scheme by replacing the terminal cost in (20) with the ADP value function approximation, leading to

		$\displaystyle\underset{\{u_{k}\}_{k=0}^{N-1}}{\text{minimize }}\hskip 2.84544pt$		$\displaystyle\sum_{k=0}^{N-1}\lVert x_{k}-x_{\mathrm{traj},k}\rVert_{Q}^{2}+\lVert u_{k}-u_{\mathrm{traj},k}\rVert_{R}^{2}$		(21)
		$\displaystyle\hskip 78.24507pt+\hat{V}^{*}_{PWM}(x_{N}-x_{\mathrm{traj},N})$
		subject to		$\displaystyle x_{k+1}=A_{k}x_{k}+B_{k}u_{k}+g_{k},$
		$\displaystyle u_{k}\in\mathcal{U}.$

In Figure 7, we show a comparison of the experimental results when using LTV-MPC-ADP, LTV-MPC-LQR and LTI-MPC-LQR, all with a horizon length of $N=1$. As can be seen in Figure 7, the initial thrust is higher for LTV-MPC-ADP, which suggests that it is able to predict the behavior of the system better than LTV-MPC-LQR. It shows that for the same horizon length, LTV-MPC-ADP reacts faster in the correction of the drop in $p_{z}$, since the initial thrust input is larger. This suggests that using the ADP value function approximation for the final cost yields better predictions and therefore, performance. The experimental comparison of LTV-MPC-ADP and LTI-MPC-LQR can be seen in the video at [26]. For details on the implementation, we refer to [23].

Figure 8 shows a series of simulations using the LTV-MPC-ADP, the LTV-MPC-LQR, and the LTI-MPC-LQR policies for horizon lengths from 1 up to 11. For every horizon length, the sum of stage costs, $\sum_{k=0}^{N}l(x_{k},u_{k})$ is computed, until all states and inputs have converged, and then plotted as a measurement of the performance of the policy. As the aim is to minimize (2), the smaller this cost is, the better the controller is performing. Figure 8 supports the results of the performance comparison given in Figure 6, as for short horizon lengths the cost of LTV-MPC-ADP is lower than the one of LTV-MPC-LQR. It also shows that the quality of the approximation of the final cost loses importance when the horizon length increases.

Figure 9 shows that the maximum implementable horizon length for the LTV-MPC-ADP policy at 50 Hz is of 4-5 steps, while the LTV-MPC-LQR policy is tractable for up to 15 steps. The online computation time of the LTV-MPC-ADP policy can be reduced by using fewer approximate value functions in $V_{PWM}^{*}$, which introduces fewer quadratic constraints in (LABEL:eq:optimization_problem_general_ADP). This generally introduces a trade-off with the online performance because the approximation quality of the cost-to-go may get reduced. Moreover, reducing the computation time allows for a higher frequency which might improve the forward Euler approximation used for the discrete time dynamics.

The offline computation time required to compute the policies LTV-MPC-LQR and LTI-MPC-LQR is solving the Riccati equation, which takes less than one second for this model. The offline computation required to prepare $\hat{V}^{*}_{PWM}$ for the LTV-MPC-ADP policy is less than 10 minutes on a standard desktop computer, using Yalmip [28] and Mosek [29] for building and solving the necessary SDPs.