<figure><img src="image_1.png" /></figure>

HAL
open science

# QP-Based Inner-Loop Control for Constraint-Safe and Robust Trajectory Tracking for Aerial Robots

Lorenzo Balandi, Paolo Robuffo Giordano, Marco Tognon

► To cite this version:

Lorenzo Balandi, Paolo Robuffo Giordano, Marco Tognon. QP-Based Inner-Loop Control for Constraint-Safe and Robust Trajectory Tracking for Aerial Robots. IEEE Robotics and Automation Letters, 2025, 10 (7), pp.6640-6647. 10.1109/LRA.2025.3568319. hal-05061950

HAL Id: hal-05061950

https://hal.science/hal-05061950v1

Submitted on 9 May 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# QP-Based Inner-Loop Control for Constraint-Safe and Robust Trajectory Tracking for Aerial Robots

Lorenzo Balandi<fnref n="1" />, Paolo Robuffo Giordano<fnref n="1" />, and Marco Tognon<fnref n="1" />

**Abstract**—Accurate trajectory tracking is crucial in aerial robotics. Optimal control methods such as Nonlinear Model Predictive Control (NMPC) are able to track trajectories exploiting the full nonlinear dynamics while respecting constraints. However, the NMPC model-based nature makes it sensitive to mismatches among nominal and real models. A common workaround to mitigate the effects of model uncertainties is to implement an inner-loop controller which robustifies the NMPC outer-loop. However, this inner-loop is usually based on purely feedback-based controllers such as PID or Incremental Nonlinear Dynamic Inversion (INDI), which do not allow to consider any constraint (such as limited actuation) or optimization criteria. In contrast, in this work we propose an optimization-based inner-loop controller inspired by Time Delay Control (TDC), that, thanks to a Quadratic Program (QP) formulation, is able to respect constrains and can thus preserve stability in presence of input saturation and model mismatches. Furthermore, thanks to the use of acceleration feedback, the knowledge of inertial parameters is not required by the proposed inner-loop which therefore makes it even more robust against model uncertainties. The overall architecture is validated on a fully-actuated hexarotor under model mismatches and aggressive trajectories. The experiments clearly show that our QP-based inner-loop improves the NMPC tracking performance while preserving the stability in conditions where a non-optimal (and more classical) inner-loop controllers would fail.

***Index Terms***—Aerial Systems: Mechanics and Control, Motion Control, Optimization and Optimal Control

## I. INTRODUCTION

PRECISE trajectory tracking controllers are crucial for Micro Aerial Vehicles (MAVs) to execute both contactless and contact-based tasks effectively [1], as they must reject external disturbances (e.g., wind) and be robust against unmodeled dynamics (e.g., aerodynamics). Over the last decade, many control strategies have been proposed to deal with the inherently unstable and nonlinear nature of MAVs, and in particular of quadrotors. For instance, a very popular choice is to rely on geometric controllers that can explicitly deal with the MAVs nonlinearities [2]. The differential flatness property has also been exploited for the tracking of aggressive trajectories [3], [4] with good results also on real experiments on quadrotors with onboard computational capabilities.

A drawback is that the control law is not able to take into account performance criteria (e.g., energy consumption)

or constraints (e.g., limited actuation); moreover, the control input is only based on the current state of the system and cannot reason about longer-term consequences of local actions (e.g., avoiding future collisions). Model Predictive Control (MPC) is a popular optimization-based control strategy that can instead explicitly handle constraints and/or optimality, and which can reason over a longer time horizon compared to a purely reactive feedback. MPC and NMPC have been successfully applied to trajectory tracking for under-actuated, fully-actuated and omnidirectional MAVs [4]–[7]. This control approach exploits a dynamic model of the system to predict its future behavior and optimize the control actions over a time horizon. This makes it possible, for instance, to effectively track unfeasible reference trajectories while respecting input and state constraints at the cost, however, of a higher computational load with respect to (wrt) simpler feedback-based controllers.

The above mentioned controls, including MPC, depend on an accurate knowledge of the robot model. To mitigate the effects of model uncertainties, robust control schemes, such as Sliding Mode [8] and $H_\infty$ [9] have been proposed in the literature. To robustify MPC, an inner-loop controller is often employed with the MPC acting as outer-loop. For this purpose, a common choice is the INDI [10], [11]: this sensor-based approach enables high-performance tracking through direct acceleration measurement or estimation and is derived from applying Nonlinear Dynamic Inversion (NDI) to an incremental system (equivalent formulations are available in the literature [10]). An alternative inner-loop controller for NMPC is $L_1$ adaptive control, successfully applied in [12],

<figure><img src="image_3.png" /><figcaption>Fig. 1: Fully-actuated hexarotor used in the experiments with reference frames.</figcaption></figure>

This work was supported by the project ANR-23-CE33-0013 "FlyHandy-Bot". Experiments presented in this paper were carried out thanks to a platform of the Robotex 2.0 French research infrastructure.

Digital Object Identifier (DOI): see top of this page.

<footnote n="1">¹ Univ Rennes, CNRS, Inria, IRISA, Rennes, 35042, France, lorenzo.balandi@inria.fr, marco.tognon@inria.fr, prg@irisa.fr.</footnote>
where the authors show that the inner-loop is able to deal with modeling errors similarly to an integral action, unlike the INDI inner-loop in [4]. However, note that different INDI formulations that more closely mirror the original one can provide the same effect [3].

Another control method similar to INDI is TDC [13], originally proposed as a possible input/output linearization method for uncertain nonlinear systems. More recent derivations of TDC can be found in [14]–[16]. The basic idea is to use sensors to measure the unknown dynamics and compensate it in an incremental fashion. TDC has been applied to robotic manipulators in [15], [16], where a discrete-time incremental system is derived and used as dynamic model for MPC. While INDI still requires a (limited) knowledge of the model, TDC is potentially *model-free* thanks to the approximation of unknown dynamics from acceleration measurements. Unlike ground manipulators [15], [16], which control joint torques directly, MAVs control propeller speeds rather than the body wrench. Therefore, in the MAVs case an approach based on TDC cannot be fully *model-free*.

All mentioned inner-loop controllers are suitable choices for increasing the robustness of the MPC outer-loop against model uncertainties thanks to their *model-free* nature (though to different extents). However, unlike MPC or other optimization-based controls, they do not offer the possibility of explicitly considering input limits or other constraints. Therefore their use as inner-loop controllers can still lead to a constraint violation and possible instability in case of model mismatches or other disturbances when used in conjunction with an outer MPC loop, event if no additional mismatches are added. In this context, the goal of this paper is to address these issues by proposing a novel high-frequency inner-loop controller that embeds TDC into a QP problem, building on the ideas introduced in [15], [16]. This results in two main benefits: (i) the robustness of the outer MPC loop against model uncertainties is still enhanced thanks to the presence of the TDC inner-loop and (ii) actuation constraints are strictly met also at the inner-loop level thanks to the QP optimization formulation, thus addressing the aforementioned shortcoming of previous inner-loop controllers.

The proposed QP-based inner-loop, referred to as *TDC-QP*, significantly reduces tracking errors without requiring precise knowledge of the MAV model parameters, showing a behavior comparable to [12]. At the same time, it ensures closed-loop stability even in the presence of input saturation—conditions where previously mentioned inner-loops would fail. These claims are further validated through the experimental results presented in Sec. V, which involve substantial model mismatches and aggressive reference trajectories that induce input saturation, demonstrating the robust performance of TDC-QP when the system operates at its limits.

The rest of the paper is structured as follows: Sec. II presents the dynamic model for the hexarotor used as case study in this work, while Sec. III illustrates the NMPC formulation. Sec. IV introduces the main contribution of the paper, that is, the derivation of an incremental system based on TDC and its embedding in a QP, as well as its practical implementation as inner-loop for NMPC and a discussion on its relation with

INDI. The experimental validation is discussed in Sec. V and Sec. VI draws the conclusions of this work.

## II. SYSTEM MODELING

We model a generic MAV with $n_p \ge 6$ propellers as a 6-DoF rigid body. To describe its motion, we define a *robot frame* $\mathcal{F}_R = \{O_R, x_R, y_R, z_R\}$ attached to the vehicle Center of Mass (CoM), and a *world frame* $\mathcal{F}_W = \{O_W, x_W, y_W, z_W\}$, with arbitrarily placed origin $O_W$, and $z_W$ opposing gravity vector. Reference frames are depicted in Fig. 1. The position of $O_R$ with respect to $\mathcal{F}_W$ is denoted as $\mathbf{p} \in \mathbb{R}^3$, while the orientation (attitude) of $\mathcal{F}_R$ wrt $\mathcal{F}_W$ is represented by the rotation matrix $\mathbf{R} = [x_R \ y_R \ z_R] \in \text{SO}(3)$, parametrized by the unit quaternion $\mathbf{q} = [q_w \ q_x \ q_y \ q_z]^\top \in \mathbb{S}(3)$. The angular velocity of $\mathcal{F}_R$ wrt $\mathcal{F}_W$, expressed in $\mathcal{F}_R$, is denoted as $\omega \in \mathbb{R}^3$, $\mathbf{v} \in \mathbb{R}^3$ is the linear velocity in $\mathcal{F}_W$.

Following the Newton-Euler formalism, expressing the translational dynamics in world frame and the rotational dynamics in robot frame, one can write the MAV model as

$$ \dot{\mathbf{p}} = \mathbf{v}, \quad \dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes [0 \ \omega^\top]^\top \quad (1a) $$

$$ \begin{bmatrix} mI_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{J} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{v}} \\ \dot{\omega} \end{bmatrix} = \begin{bmatrix} -mgz_W \\ -\omega \times \mathbf{J}\omega \end{bmatrix} + c_f \begin{bmatrix} \mathbf{R} & \mathbf{0}_3 \\ \mathbf{0}_3 & I_3 \end{bmatrix} \mathbf{G} \mathbf{w}^{\circ 2} \quad (1b) $$

where $\otimes$ is the quaternion product, $m$ is the robot mass, $-gz_W$ is the gravity vector, $\mathbf{J}$ is the constant inertia matrix of the robot, $c_f$ is the thrust coefficient, and $I_3$ and $\mathbf{0}_3$ are the $3 \times 3$ identity and zero matrices respectively. The term multiplying the linear/angular accelerations in (1b) is called generalized inertia matrix and will be denoted as $\mathbf{M}$ in the following derivations. Vector $\mathbf{w}^{\circ 2}$ is the component-wise square of the rotor speeds $\mathbf{w} \in \mathbb{R}^{n_p}$ and will be taken as *control input*. As for the quantity $\mathbf{G} \in \mathbb{R}^{6 \times n_p}$, it represents the allocation matrix computed as in [6]. By denoting $\gamma = c_f \mathbf{w}^{\circ 2}$ as the vector of propeller thrusts, the quantity $\mathbf{G}\gamma = [\mathbf{f}^\top \tau^\top]^\top \in \mathbb{R}^6$ is the body wrench. Note that for fully-actuated aerial vehicles, like the one considered in this work, $\mathbf{G}$ is full rank, while for standard quadrotors its rank is limited to 4.

The value of the various parameters in (1) (center of mass, geometrical quantities, thrust coefficient, etc) is in general only approximately known. Therefore, we introduce a *nominal* dynamic model

$$ \begin{bmatrix} \hat{m}I_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \hat{\mathbf{J}} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{v}} \\ \dot{\omega} \end{bmatrix} = \begin{bmatrix} -\hat{m}gz_W \\ -\omega \times \hat{\mathbf{J}}\omega \end{bmatrix} + \hat{c}_f \begin{bmatrix} \mathbf{R} & \mathbf{0}_3 \\ \mathbf{0}_3 & I_3 \end{bmatrix} \hat{\mathbf{G}} \mathbf{w}^{\circ 2} \quad (2) $$

where the $\hat{\cdot}$ indicates the approximated knowledge of uncertain parameters due to measurement or estimation errors. The estimated generalized inertia matrix will be denoted as $\hat{\mathbf{M}}$. This nominal model will be relevant in the following developments.

### A. Problem Statement and Control Framework Overview

The goal of the control framework discussed in the following is to track a desired state trajectory for the robot under model uncertainties. To this end, an NMPC is proposed based on the *nominal model* (2). Given the mismatch between real and nominal dynamical models (1)–(2), the NMPC will present
considerable tracking errors due to its model-based nature. A TDC-based controller is thus developed for fully-actuated MAVs and integrated in the control scheme as inner-loop for the NMPC. This additional loop computes control inputs for the MAV given states and inputs from the NMPC and reduces the tracking error thanks to acceleration feedback that mitigates the effects of model uncertainties. However, the TDC inner-loop is not able to handle constraints and, thus, even if the inputs generated by the NMPC would respect actuation limits the TDC action can still incur into input saturation and introduce instability in presence of model mismatches. To solve this issue, we then embed the TDC inner-loop into a QP which guarantees the satisfaction of the same input constraints as in the NMPC outer-loop.

### III. NONLINEAR MODEL PREDICTIVE CONTROLLER

We rely on a NMPC for solving the trajectory tracking task. NMPC iteratively solves a constrained Optimal Control Problem (OCP) and computes optimal states and inputs over a (finite) prediction horizon by forward integrating the MAV nominal model (2). In our case, the state and input vectors are $x = [p^\top q^\top v^\top \omega^\top]^\top \in \mathbb{R}^3 \times \mathbb{S}(3) \times \mathbb{R}^6$ and $u = w^{\circ 2} \in \mathbb{R}^{n_p}$, respectively. The control objective is to track as close as possible a desired time-varying state trajectory $x_d(t) = [p_d(t)^\top q_d(t)^\top v_d(t)^\top \omega_d(t)^\top]^\top$, which may not be dynamically feasible considering actuation constraints, and possibly a reference input $u_d(t)$ computed by a trajectory planner or set to a constant value (e.g., zero or hovering value).

In order to numerically solve the OCP, states and inputs are typically discretized using $N$ equally spaced intervals with discretization step $dt$ and denoted with subscript $\cdot_k$. The references are also sampled and fed to the NMPC accordingly, obtaining the following Nonlinear Program (NLP):

$$ \min_{\substack{x_0, \dots, x_N, \\ u_0, \dots, u_{N-1}}} \sum_{k=0}^{N-1} \left( \begin{aligned} & (\|x_k - x_{k,d}\|_{2,Q}^2 + \|u_k - u_{k,d}\|_{2,U}^2) \\ & + \|x_N - x_{N,d}\|_{2,Q_N}^2 \end{aligned} \right) \quad (3a) $$

$$ \text{subject to} \quad x_{k+1} = \hat{f}(x_k, u_k) \quad k \in [0, N-1] \quad (3b) $$

$$ x_0 = \hat{x} \quad (3c) $$

$$ u_{min} \le u_k \le u_{max} \quad k \in [0, N-1], \quad (3d) $$

where $Q = \text{diag}(Q_p, Q_q, Q_v, Q_\omega)$, $Q_* \in \mathbb{R}^{3 \times 3}$ and $U \in \mathbb{R}^{n_p \times n_p}$ are positive definite diagonal weight matrices, $Q_N = Q$ (we are equally interested in all nodes), constraint (3b) is the discretized version of the nominal dynamics (2), $\hat{x}$ is the current robot state (assumed available from state estimation), $u_{min}$ and $u_{max}$ are the minimum and maximum squared rotors velocities, respectively. For the attitude error, we use the vectorial part of the quaternion error with the following abuse of notation as in [4]:

$$ q_{err}^d = q - q_d = (q \otimes q_d^{-1})_{x,y,z}. \quad (4) $$

### IV. INNER-LOOP CONTROLLER

Here we derive a TDC for fully-actuated MAVs and discuss its similarities with INDI. We then show how to embed the

TDC in a QP with actuation constraints and integrate the inner-loop controller with the NMPC outer-loop. Finally, we discuss practical considerations for implementation.

#### A. TDC for Fully-actuated MAVs

We report here the application of TDC to fully-actuated MAVs in order to obtain an incremental system where uncertain dynamics terms are approximated with sensors measurements, following the procedure explained in [15], [16]. We demonstrate that, in the case of MAVs, knowledge of inertial parameters is unnecessary; however, the approach still depends on the allocation matrix, specifically on certain geometrical parameters and propeller coefficients.

The MAV dynamics (1) can be rewritten as a nonlinear system in control affine form

$$ \dot{x}_1 = h(x_1, x_2) \quad (5a) $$

$$ \dot{x}_2 = f(x) + F(x)u, \quad (5b) $$

where $x_1 = [p^\top q^\top]^\top \in \mathbb{R}^3 \times \mathbb{S}(3)$, $x_2 = [v^\top \omega^\top]^\top \in \mathbb{R}^6$, $u = w^{\circ 2} \in \mathbb{R}^{n_p}$, $x = [x_1^\top x_2^\top]^\top$ and $h(\cdot)$ is a function expressing the non-linearity of the quaternion kinematics (1a). Moreover,

$$ f(x) = M^{-1} \begin{bmatrix} -mqz_W \\ -\omega \times J\omega \end{bmatrix}, \quad F(x) = M^{-1}c_f \bar{R}G, \quad (6) $$

with $\bar{R} = \text{diag}(R, I_3)$. Introducing matrix

$$ \tilde{G} = \tilde{G} \bar{R} \tilde{G} \in \mathbb{R}^{6 \times n_p} \quad (7) $$

where $\tilde{G} \in \mathbb{R}^{6 \times 6}$ is a constant diagonal positive-definite gain matrix and $\tilde{G}$ the best approximation of the allocation matrix, (5b) can be rewritten as

$$ \tilde{G}^+ \dot{x}_2 = H(x, \dot{x}) + u, \quad (8) $$

with $H(x, \dot{x}) = (\tilde{G}^+ - F^+(x))\dot{x}_2 + F^+(x)f(x) \in \mathbb{R}^{n_p}$. If $n_p > 6$, $\tilde{G}$ and $F$ are not square and their Moore-Penrose pseudoinverse $\tilde{G}^+$, $F^+ \in \mathbb{R}^{n_p \times 6}$ must be used.

The purpose of matrix $\tilde{G}$ is not only to provide a tunable gain for the body wrench, but also to approximate the thrust coefficient and inverse of generalized inertia matrix. The quantity $H(x, \dot{x})$ includes the terms $f(x)$ and $F(x)$ which are assumed unknown or uncertain. According to [15], [16], these terms can be approximated using acceleration measurements. Considering a sufficiently small time interval $\Delta t$, the value of $H(x, \dot{x})$ at time $t$ can be considered close to its sampled value at time $t - \Delta t$. Therefore, the uncertain terms in (8) can be approximated using sensors, state estimation algorithms and input measurements, obtaining the Time Delay Estimation (TDE) of $H(x, \dot{x})$ as

$$ \hat{H}(x, \dot{x}) = \tilde{G}^+ \dot{x}_{2,0} - u_0, \quad (9) $$

where $\dot{x}_{2,0} = \dot{x}_2(t - \Delta t)$ and $u_0 = u(t - \Delta t)$. Note that (9) uses the most recent sampled values of accelerations $\dot{x}_2$ and inputs $u$ to approximate the unknown dynamics, thus making this method a sensor-based approach. Following the TDC literature, $\Delta t$ is the sensors sampling time (1 ms in our case.)
By replacing $H(x, \dot{x})$ in (8) with its TDE computed in (9), we obtain the incremental dynamics

$$ \dot{x}_2 = \dot{x}_{2,0} + \tilde{G}(u - u_0) = \dot{x}_{2,0} + \tilde{G}\Delta u, \quad (10) $$

which will be used in the QP formulation. Vector $\Delta u$ is the incremental input. Note that (10) does not depend on the generalized mass matrix $M$ anymore, but there remains a dependency on the allocation matrix $\tilde{G}$ through $\tilde{G}$. TDC applied to MAVs is therefore not completely model-free because of the dependency on geometrical quantities and the ratio between drag and thrust through $\tilde{G}$ (see [6], Section 2.1). As previously mentioned, this limitation is due to the fact that the measured variables are linear acceleration and angular velocity whereas the controlled ones are the motor rotational speeds instead of the body-frame wrench.

Furthermore, in (10) we omitted the TDE error, see [15] and [16] for a complete discussion. Gain $\tilde{G}$ is tuned in order to satisfy a condition which guarantees the TDE error boundedness, as discussed in [13] Sec. III, [15] Sec. II-B and [16] Sec. III-A. As general suggestions to select this parameter, consider that large $\tilde{G}$ results in large TDE errors, while small $\tilde{G}$ causes noisy responses. To tune $\tilde{G}$, we can start with a sufficiently large positive value that guarantees stability and then decreases it until the closed loop response starts to become noisy. Indeed, this manual tuning procedure is in practice limited by the sensors noise level.

Note that our derivation applied to the MAV case slightly misuses the term TDC, because $\tilde{G}$ depends on the state via the rotation matrix in $\tilde{R}$. However, since a rotation matrix has unitary norm and only changes the direction of a vector and not its magnitude, we still consider it a TDC. The key property of fixed, tunable gain remains by assuming no uncertainty in the attitude provided by the state estimator.

By solving (10) for the input $u$, one obtains

$$ u = u_0 + \tilde{G}^{+}(\dot{x}_2 - \dot{x}_{2,0}), \quad (11) $$

where the acceleration $\dot{x}_2$ includes the reference acceleration and a stabilizing PD action [13], [11], [14], [15] as follows

$$ \dot{x}_2 = \nu = \dot{x}_2^{\text{ref}} + \begin{bmatrix} K_p(p_{\text{ref}} - p) + K_v(v_{\text{ref}} - v) \\ K_q q_{\text{err}}^{\text{ref}} + K_{\omega}(\omega_{\text{ref}} - \omega) \end{bmatrix} \quad (12) $$

with $K_*$ being suitable constant diagonal gain matrices and $q_{\text{err}}^{\text{ref}}$ similar to (4). Note that this PD action is crucial to compensate model mismatches [17], as shown by experimental results. (12) allows tracking a reference state computed interpolating optimal states and inputs from NMPC (3), as detailed in Sec. IV-D. Control law (11) can be used as inner-loop controller to enhance the NMPC tracking capabilities and, in particular, increase its robustness against model uncertainties.

## B. Relations with INDI

The control law in (11) closely resembles the INDI law, which, when applied to the fully-actuated MAV case, can be expressed as:

$$ u = u_0 + \tilde{G}(x_0)^{+}(\dot{x}_2 - \dot{x}_{2,0}), \quad (13) $$

where $\tilde{G}(x_0) = \tilde{G}(x_0)\tilde{R}\tilde{G}$ and $\dot{x}_2 = \nu$. A nice and detailed comparison between TDC and INDI can be found in [14]. In

general, INDI can be viewed as TDC control with a state-dependent gain $\tilde{G}(x_0)$ in place of $\tilde{G}$. There exist several derivations of INDI in the literature, see for example [3], [10], [14]. INDI is based on a time-scale separation argument, stating that the input rate of change is much faster than the state rate of change. This assumption is absolutely reasonable when INDI is applied as fast inner-loop. The self-scheduling property of INDI could enhance system stability. However, for MAVs with fixed geometry and inertial parameters, INDI would have a non-tunable gain, making it less flexible and more dependent on hard-to-obtain parameters such as inertia [3], [4]. Moreover, it could result in very aggressive gains which are not applicable in practice due to, e.g., sensor noise.

For this reason we believe that, if applied to MAVs with constant parameters, TDC is more advisable, as it allows greater tuning flexibility through the gain matrix $\tilde{G}$ without requiring the knowledge of mass and inertia. This is similar to the integral gain approach presented in [17], and a relation between TDC and the gains of a PID in incremental form is reported in [14].

## C. QP-based TDC

To make the inner-loop controller able to respect constraints, the incremental dynamics (10) is embedded in a QP

$$ \min_{\Delta u} \quad \|\dot{x}_2 - \nu\|_{2, Q_2}^2 + \|\Delta u\|_{2, U_2}^2 \quad (14a) $$

$$ \text{s.t.} \quad u_{\min} \le u_0 + \Delta u \le u_{\max}, \quad (14b) $$

where $Q_2 \in \mathbb{R}^{6 \times 6}$ and $U_2 \in \mathbb{R}^{n_p \times n_p}$ are positive definite diagonal matrices, $\dot{x}_2$ is the incremental dynamics (10). In (14), the objective is to track an acceleration $\nu$ defined in (12) while minimizing the incremental input and keeping the full input between minimum and maximum limits (the same limit of the NMPC, see (3d)).

By expanding the cost function (14a) and reformulating the constraints, (14) is rewritten as

$$ \min_{\Delta u} \quad \Delta u^{\top} H \Delta u + g^{\top} \Delta u \quad (15) $$

$$ \text{s.t.} \quad A \Delta u \le b $$

where

$$ H = \tilde{G}^{\top} Q_2 \tilde{G} + U_2, \quad g = 2Q_2^{\top} \tilde{G}^{\top} (\dot{x}_{2,0} - \nu) \quad (16) $$

$$ A = [-I \ I]^{\top}, \quad b = [(u_0 - u_{\min})^{\top} (-u_0 + u_{\max})^{\top}]^{\top}. \quad (17) $$

Letting $\Delta u^*$ be the solution of (15), the input to apply to the system at the next step is

$$ u^* = u_0 + \Delta u^*, \quad (18) $$

which is a vector of $n_p$ squared propeller velocities consistent with the constraint imposed by the NMPC. We call the inner-loop based on (14) TDC-QP.

## D. Application to NMPC and Practical Implementation

We explain here how we obtain the reference state and acceleration to be tracked by the inner-loop (computed using (11) or QP (14)). The overall scheme for the TDC-QP inner-loop case is depicted in Fig. 2, where $a \in \mathbb{R}^3$ indicates the linear acceleration, namely $\dot{v} = a$. If the QP is not used,
<figure><img src="image_4.png" /><figcaption>Fig. 2: Control scheme: NMPC outer-loop and TDC-QP inner-loop.</figcaption></figure>

the corresponding block in Fig. 2 and the sum are replaced by Equation (11). The reference values are obtained from NMPC as follows: at each NLP solution of (3), the algorithm outputs optimal states and inputs for all the shooting nodes along the prediction horizon, namely $x_k^{opt}$ for $k = 0, \dots, N$ and $u_k^{opt}$ for $k = 0, \dots, N-1$, where $x_0^{opt}$ is the same of (3c) and $u_0^{opt}$ is the input that should be applied to the system without inner-loop controller. Since the temporal interval between two consecutive NMPC prediction nodes contains multiple executions of the inner-loop, optimal states and inputs are linearly interpolated between the first and the second shooting nodes, namely between ($x_0^{opt}$, $u_0^{opt}$) and ($x_1^{opt}$, $u_1^{opt}$) considering the current time. In Fig. 2, $R_{opt}$ and $u_{opt}$ represent attitude and input computed via interpolation, respectively. Linear and angular reference accelerations $\dot{x}_2^{ref}$ are computed using optimal interpolated states and inputs and inverting the model (2), as shown in Fig. 2.

Note that the *desired* trajectory has no feasibility guarantees and it is filtered by the NMPC to produce feasible (according to nominal model (2)) states and controls, while *reference* trajectory to be tracked by the inner-loop controller is computed through NMPC data and thus inherits the feasibility property.

In practical implementations, signal filtering is crucial since signals from onboard sensors such as the IMU are very noisy. As discussed in [3], [4], [10], all feedback signals must be low-pass filtered with the same filter in order to accumulate the same amount of delay. In Fig. 2, the block LPF indicates a second order Butterworth filter with cutoff frequency $f$, while low-pass filtered signals are indicated with subscript $\cdot f$. Moreover, the angular acceleration in (10) (last three elements of $\dot{x}_{2,0}$) cannot be directly measured by sensors and it is thus obtained by numerically differentiating the filtered angular velocity. Since this operation amplifies noise, a good filtering is essential.

Concerning the input feedback $u_0$ in (10), if it cannot be measured (or it cannot be measured fast enough, as in our case), it is necessary to estimate it from previous control commands considering the actuation delay as explained in [10]. This is just a technological limitation which can be solved using different hardware. In this work, we achieve this by applying a first-order filter with a time constant $\tau_1$ to the most recently computed input, which yields the discrete-time transfer function $A(z)$. The estimated input, according to [10], is further filtered with the same filter used for the accelerations in order to synchronize the signals.

Finally, since the computed input $u^*$ is a vector of $n_p$ squared propeller velocities, we take the elementwise square root to obtain the final command sent to low-level electronics.

## V. EXPERIMENTS

We present here the experimental setup, the details of the implementation on onboard hardware and the final results. The purpose of the experiments is to show that the proposed TDC-QP control scheme offers the same advantages of other inner-loop controllers for NMPC while guaranteeing stability in case of saturation and model mismatches, the latter condition being unavoidable in real robots. The videos related to the proposed experiments can be found in the multimedia material.

### A. Experimental Setup

The proposed control algorithm is tested on the fully-actuated hexarotor with fixedly tilted propellers shown in Fig. 1 whose design has been introduced in [18]. The platform weights 3.05 kg including batteries and $n_p = 6$. Note that this vehicle is fully-actuated and not omnidirectional, and the forces that it can exert laterally (in the $x_R-y_R$ plane) are limited and smaller than the one along $z_R$, as clearly visible in the experimental part of [19], where the same robot is used. The robot is equipped with a NUC13 onboard computer running Ubuntu. The flight controller runs custom firmware and is equipped with IMU (accelerometer and gyroscope). The experiments are performed in an indoor arena equipped with motion capture system providing position and attitude data at 200 Hz which are fused with IMU data sampled at 1 kHz by the state estimator. As previously mentioned, the final command sent to low-level electronics are desired rotors speeds, which are tracked in closed-loop by a control algorithm [20] embedded in the Electronic Speed Controllers (ESCs).

The robot is controlled to track a trajectory which may be unfeasible (i.e., lead to input saturation) due to excessive accelerations or to heavy model mismatches. Note that minimal model mismatches are always present, even in nominal conditions, because of air turbulences and imprecise assembly.

### B. Implementation Details

The NMPC is implemented with acados [21] Python interface using the Real Time Iteration (RTI) scheme [22], and runs on the high-level onboard computer at 100 Hz with a 1 s prediction horizon and 20 shooting nodes. Nominal dynamics (2) is discretized using a fourth order Runge-Kutta integrator. State estimation, inner-loop controller (TDC and TDC-QP), and interfaces with a motion tracking system (mocap) and low-level electronics run onboard at 1 kHz within the open-source framework Telekyb3¹. The control algorithm described in this paper is implemented in a novel component called *tdopt*, where PROXQP [23] is used as QP solver. In order to compare the performance of different controllers, it is possible to use TDC inner-loop control without QP computing the input with (11) or to not use the inner-loop at all. The first option is comparable to INDI as discussed in Sec. IV-B, while in the latter case the control input computed by (3) is directly sent to low-level electronics.

The control gains and parameters used for the experiments are reported in Table I, where the gain matrix has been divided

<footnote n="1">¹https://git.openrobots.org/projects/telekyb3</footnote>
<table><caption>TABLE I: Control gains and parameters.</caption><thead><tr><th>Control</th><th>Param.</th><th>Value</th><th>Control</th><th>Param.</th><th>Value</th></tr></thead><tbody><tr><td>NMPC</td><td><b>Q</b><sub>p</sub></td><td>250 · <b>I</b><sub>3</sub></td><td>TDC-QP</td><td><b>g̃</b><sub>f<sub>x,y</sub></sub></td><td>0.003 · <b>I</b><sub>2</sub></td></tr><tr><td>NMPC</td><td><b>Q</b><sub>q</sub></td><td>10 · <b>I</b><sub>3</sub></td><td>TDC-QP</td><td><b>g̃</b><sub>f<sub>z</sub></sub></td><td>0.0006</td></tr><tr><td>NMPC</td><td><b>Q</b><sub>v</sub></td><td>5 · <b>I</b><sub>3</sub></td><td>TDC-QP</td><td><b>g̃</b><sub>τ</sub></td><td>0.03 · <b>I</b><sub>3</sub></td></tr><tr><td>NMPC</td><td><b>Q</b><sub>ω</sub></td><td>0.1 · <b>I</b><sub>3</sub></td><td>TDC-QP</td><td><b>K</b><sub>p</sub></td><td>9 · <b>I</b><sub>3</sub></td></tr><tr><td>NMPC</td><td><b>U</b></td><td>10<sup>-8</sup> · <b>I</b><sub>6</sub></td><td>TDC-QP</td><td><b>K</b><sub>q</sub></td><td>9 · <b>I</b><sub>3</sub></td></tr><tr><td>NMPC</td><td><i>N</i></td><td>20</td><td>TDC-QP</td><td><b>K</b><sub>v</sub></td><td>4.242 · <b>I</b><sub>3</sub></td></tr><tr><td>NMPC</td><td><i>dt</i></td><td>50 ms</td><td>TDC-QP</td><td><b>K</b><sub>ω</sub></td><td>4.242 · <b>I</b><sub>3</sub></td></tr><tr><td>TDC-QP</td><td><b>Q</b><sub>2</sub></td><td><b>I</b><sub>6</sub></td><td>TDC-QP</td><td><i>f</i></td><td>5 Hz</td></tr><tr><td>TDC-QP</td><td><b>U</b><sub>2</sub></td><td>10<sup>-8</sup> · <b>I</b><sub>6</sub></td><td>TDC-QP</td><td><i>τ</i><sub>1</sub></td><td>0.02</td></tr></tbody></table>

in $\bar{G} = \text{diag}(\bar{g}_{f_{x,y}}, \bar{g}_{f_{z}}, \bar{g}_{\tau})$ in order to distinguish gain in horizontal force, vertical force and torque, respectively. The desired input $u_{k,d}$ in cost function (3a) is set to the hovering value.

### C. Experimental Results

We performed a set of experiments with different conditions in order to validate the proposed TDC-QP inner-loop and compare it to other controllers. For each scenario (numbered from 1 to 4), three controls are tested and compared: NMPC without inner-loop control; NMPC + TDC inner-loop without QP computed with (11); NMPC + TDC-QP inner-loop computed with (15), with results in Table II. After a gain tuning process, to ensure comparable performance to TDC-QP, the second controller is implemented with $\bar{g}_{\tau} = 0.015 \mathbf{I}_3$ while the other gains are as in Table I. Its limitations are still highlighted by experiments with input saturation.

The trajectory chosen for the experiments is a double symmetric chirp along $y_W$. This seems an appropriate choice since it can heavily affect also the attitude of the MAV and it allows to trigger input saturation even in a small space by properly changing its parameters. More in detail, the desired position is $\mathbf{p}_d(t) = [0 \ p_y(t) \ \bar{z}]^\top$, where $\bar{z}$ is the desired takeoff height and

$$p_y(t) = A \sin(\xi(t)t), \quad \xi(t) = \begin{cases} \xi t & \text{if } t \in [0, \frac{\bar{t}}{2}] \\ \xi(\bar{t} - t) & \text{if } t \in [\frac{\bar{t}}{2}, \bar{t}] \end{cases} \quad (19)$$

where $A$ is the amplitude and $\xi$ is the slope used to transition from the initial frequency $\xi(0) = 0$ Hz to $\xi(\frac{\bar{t}}{2}) = f_m$ to finally $\xi(\bar{t}) = 0$ Hz.

The desired velocity $v_d(t)$ is defined accordingly with the position, while desired attitude and angular velocities are set to zero. However, NMPC cost function weights give more importance to position tracking than attitude tracking, so the hexarotor tilts when the required lateral force exceeds the platform bounds.

By varying chirp parameters and experiments conditions, we consider four scenarios with results reported in Table II and discussed in the following. In Table II, columns "M" and "S" specify respectively if model mismatches have been added to the hexarotor beside the minimal ones and if it reaches heavy input saturation. The chirp amplitude is $A = 1$ m for each scenario. The controllers performance are evaluated computing the RMS values of position tracking error $e_p$ and attitude tracking error converted as roll, pitch and yaw errors $e_q = [e_\phi \ e_\theta \ e_\psi]^\top$ for better clarity. The input is depicted

normalized dividing by its maximum achievable value. The red dashed lines in the input plots represents minimum and maximum values. A red background in input plots indicates input saturation. Motors are numbered from 1 to 6 counter-clockwise, with motor 1 on the positive $x_R$ axis. Takeoff and landing are not depicted in the plots and not considered for errors computation.

1) **Scenario 1 - nominal conditions:** we first test the algorithms without additional model mismatches. Only minimal (unavoidable) mismatches are present and there is no input saturation. The three controllers perform similarly and they all preserve system stability. From Table II and Fig. 3 we can see that, even if there are turbulences due to air bouncing against the walls, the NMPC is able to reject the unknown disturbance also without inner-loop controller. However, there is a constant error in yaw which is compensated with both inner-loop controllers. The reason for this yaw offset may be the imperfect assembly of the hexarotor, which does not lead to a perfect compensation of the internal forces. In general, from the RMS errors, it is clear that an inner-loop controller decreases the attitude error, as the inertia is one of the most uncertain parameters in MAVs, while the mass is easier to measure. The fact that constant errors are reduced to zero suggests that acceleration-based inner-loop controllers (both with and without QP) embed an integral action, as discussed in [14], [17]. Lastly, another advantage of the TDC-QP inner-loop is that it leads to more uniform inputs as shown in Fig. 3(b), because it penalizes high input increments.

2) **Scenario 2 - aggressive trajectory:** we test the controllers with a trajectory which leads to input saturation without additional model mismatches beside the minimal ones. As shown in Table II and Fig. 4, NMPC and NMPC+TDC-QP performs similarly, with the advantage of the latter of reducing attitude and vertical position errors as in previous experiments. This control scheme is again stable despite the fact it reaches saturation multiple times, as depicted in Fig. 4(d) and Fig. 4(b). The TDC inner-loop without QP becomes unstable in the most aggressive part of the trajectory and the experiment is manually interrupted (red background in Fig. 4(c)). In an ideal case (achievable only in simulation) this situation

<figure><img src="image_5.png" /><figcaption>Fig. 3: Scenario 1 with no saturation nor added mismatches.</figcaption></figure>
<table><caption>TABLE II: Experiments results for the tested scenarios and controllers.</caption><thead><tr><th>Scenario</th><th>M</th><th>S</th><th>f<sub>m</sub> [Hz]</th><th>t [s]</th><th>Tracking error</th><th>NMPC</th><th>NMPC+TDC no QP</th><th>NMPC+TDC-QP</th></tr></thead><tbody><tr><td>1</td><td>✗</td><td>✗</td><td>0.40</td><td>20.0</td><td>e<sub>p,RMS</sub> [m]<br>e<sub>q,RMS</sub> [deg]</td><td>[0.013, 0.032, 0.009]<sup>†</sup><br>[10.133, 1.047, 3.876]<sup>†</sup></td><td>[0.014, 0.028, 0.010]<sup>†</sup><br>[10.244, 0.618, 0.372]<sup>†</sup></td><td>[0.007, 0.030, 0.006]<sup>†</sup><br>[9.820, 0.283, 0.261]<sup>†</sup></td></tr><tr><td>2</td><td>✗</td><td>✓</td><td>0.52</td><td>26.9</td><td>e<sub>p,RMS</sub> [m]<br>e<sub>q,RMS</sub> [deg]</td><td>[0.013, 0.040, 0.021]<sup>†</sup><br>[15.430, 1.350, 4.443]<sup>†</sup></td><td>CRASH<br>CRASH</td><td>[0.012, 0.041, 0.018]<sup>†</sup><br>[14.956, 0.458, 0.438]<sup>†</sup></td></tr><tr><td>3</td><td>✓</td><td>✗</td><td>0.40</td><td>20.0</td><td>e<sub>p,RMS</sub> [m]<br>e<sub>q,RMS</sub> [deg]</td><td>[0.012, 0.032, 0.026]<sup>†</sup><br>[10.673, 1.631, 5.230]<sup>†</sup></td><td>[0.013, 0.029, 0.011]<sup>†</sup><br>[10.443, 0.552, 0.384]<sup>†</sup></td><td>[0.013, 0.029, 0.011]<sup>†</sup><br>[10.104, 0.558, 0.385]<sup>†</sup></td></tr><tr><td>4</td><td>✓</td><td>✓</td><td>0.40</td><td>20.0</td><td>e<sub>p,RMS</sub> [m]<br>e<sub>q,RMS</sub> [deg]</td><td>[0.007, 0.033, 0.030]<sup>†</sup><br>[10.734, 2.107, 5.200]<sup>†</sup></td><td>CRASH<br>CRASH</td><td>[0.012, 0.032, 0.008]<sup>†</sup><br>[10.387, 0.989, 0.871]<sup>†</sup></td></tr></tbody></table>

<figure><img src="image_6.png" /><figcaption>Fig. 4: Scenario 2 with aggressive trajectory, no added mismatches (only minimal ones).</figcaption></figure>

would not lead to a crash, because the commands from NMPC would be tracked without exceeding input limitations as real and nominal models coincide. This experiment highlights the importance of a *safe* inner-loop such as TDC-QP even with minimal, unavoidable mismatches. To further investigate and validate this, we tested the same controller in a simulation where nominal and real models coincides and the inner-loop controller without QP had comparable performance to TDC-QP, maintaining stability.

3) **Scenario 3 - additional model mismatches:** to add model mismatches to the minimal ones, a 350 g mass is attached with a rope near the CoM of the robot and acts as a pendulum. The chirp trajectory is the same of scenario 1. From Table II one can notice that, since the NMPC is not aware of the added mass, it has a constant offset in the vertical position and the yaw error is increased wrt experiment 1. The results show that the TDC-based inner-loop controllers (with and without QP) are able to greatly reduce these offsets. In particular, the two inner-loops perform very similarly on both position and attitude trackings. These results are similar to Scenario 1 and for space reasons we do not show the plots here.

4) **Scenario 4 - heavy model mismatches and saturation:** we test here the advantage of TDC-QP inner-loop control in case of model mismatch and trajectory leading to heavy input saturation. The benefits of inner-loop tracking are already known, but we show here how TDC-QP is able to provide lower tracking errors *while* preserving system stability also in this challenging conditions, which is the main focus of this

paper. These conditions may represent a system of which a very coarse model is available. The desired chirp trajectory is the same of scenario 1 and 3. To increase the model mismatch beyond the minimal one, some masses are attached to the hexarotor with ropes in a pendulum-like style. In particular, there are a 300 g mass attached slightly below the CoM, and a 220 g mass attached along the $x_R$ axis to the arm connecting motor 1 to the frame, as shown in Fig. 5(a). Note that the masses act as a pendulum and they introduce not only unknown time-varying forces, but also torques. From Table II and Fig. 5, it is evident that the TDC-QP inner-loop is able to greatly reduce the tracking errors, in particular in vertical position and yaw, while handling the saturation of the input. TDC without QP, instead, becomes unstable and leads to a crash (red background in Fig. 5(c)). These behaviors are also shown in the attached video.

## VI. CONCLUSION AND FUTURE WORK

Tracking aggressive trajectories under strong model uncertainties still represents a challenge in Aerial Robotics, as common inner-loop controllers used to improve accuracy can cause input saturation and, thus, instability. To address this, we propose a TDC-based fast inner-loop for NMPC trajectory tracking. Our approach retains the benefits of acceleration-based inner-loop controllers *while* ensuring actuation constraints align with the outer NMPC loop, preserving stability under saturation and model mismatches. This is achieved by formulating dynamics in incremental form and embedding it
<figure><img src="image_7.png" /><figcaption>Fig. 5: Scenario 4 with heavy added mismatches and saturation.</figcaption></figure>

in a QP, considering filters delays and interpolating NMPC data. Experiments on a fully-actuated hexarotor demonstrate superior safety performance in challenging conditions, handling input saturation where others fail.

In the future, we plan to apply TDC-QP inner-loop to platforms such as torque-controlled manipulators and aerial manipulators, where we could achieve *model-free* control [16]. Since in those systems the inertia varies with the state, a comparison with INDI inner-loop could be performed. As hardware improvements, to enhance inner-loop reliability, we will further investigate vibration damping on IMU and use more performant ESCs capable of providing fast rotors speed feedback. This will eliminate the need to estimate the actuation delays in the input feedback.

## REFERENCES

[1] A. Ollero, M. Tognon, A. Suarez, D. Lee, and A. Franchi, "Past, present, and future of aerial robotic manipulators," *IEEE Transactions on Robotics*, vol. 38, no. 1, pp. 626-645, 2021.

[2] T. Lee, M. Leok, and N. H. McClamroch, "Geometric tracking control of a quadrotor uav on se (3)," in *49th IEEE conference on decision and control (CDC)*. IEEE, 2010, pp. 5420-5425.

[3] E. Tal and S. Karaman, "Accurate tracking of aggressive quadrotor trajectories using incremental nonlinear dynamic inversion and differential flatness," *IEEE Transactions on Control Systems Technology*, vol. 29, no. 3, pp. 1203-1218, 2020.

[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 Transactions on Robotics*, vol. 38, no. 6, pp. 3357-3373, 2022.

[5] H. Nguyen, M. Kamel, K. Alexis, and R. Siegwart, "Model predictive control for micro aerial vehicles: A survey," in *2021 European Control Conference (ECC)*. IEEE, 2021, pp. 1556-1563.

[6] D. Bicego, J. Mazzetto, R. Carli, M. Farina, and A. Franchi, "Nonlinear model predictive control with enhanced actuator model for multi-rotor aerial vehicles with generic designs," *Journal of Intelligent & Robotic Systems*, vol. 100, no. 3, pp. 1213-1247, 2020.

[7] L. Peric, M. Brunner, K. Bodie, M. Tognon, and R. Siegwart, "Direct force and pose nmpc with multiple interaction modes for aerial push-and-slide operations," in *2021 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2021, pp. 131-137.

[8] K. D. Young, V. I. Utkin, and U. Ozguner, "A control engineer's guide to sliding mode control," *IEEE transactions on control systems technology*, vol. 7, no. 3, pp. 328-342, 1999.

[9] T. Başar and P. Bernhard, *H-infinity optimal control and related minimax design problems: a dynamic game approach*. Springer Science & Business Media, 2008.

[10] E. J. Smeur, Q. Chu, and G. C. De Croon, "Adaptive incremental nonlinear dynamic inversion for attitude control of micro air vehicles," *Journal of Guidance, Control, and Dynamics*, vol. 39, no. 3, pp. 450-461, 2016.

[11] X. Wang, E.-J. Van Kampen, Q. Chu, and P. Lu, "Stability analysis for incremental nonlinear dynamic inversion control," *Journal of Guidance, Control, and Dynamics*, vol. 42, no. 5, pp. 1116-1129, 2019.

[12] D. Hanover, P. Foehn, S. Sun, E. Kaufmann, and D. Scaramuzza, "Performance, precision, and payloads: Adaptive nonlinear MPC for quadrotors," *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 690-697, 2021.

[13] K. Youcef-Toumi and S.-T. Wu, "Input/output linearization using time delay control," 1992.

[14] B. P. Acquatella and Q. P. Chu, "Agile spacecraft attitude control: An incremental nonlinear dynamic inversion approach," *IFAC-PapersOnLine*, vol. 53, no. 2, pp. 5709-5716, 2020.

[15] Y. Wang, M. Leibold, J. Lee, W. Ye, J. Xie, and M. Buss, "Incremental model predictive control exploiting time-delay estimation for a robot manipulator," *IEEE Transactions on Control Systems Technology*, vol. 30, no. 6, pp. 2285-2300, 2022.

[16] Y. Wang, Y. Liu, M. Leibold, M. Buss, and J. Lee, "Hierarchical incremental MPC for redundant robots: a robust and singularity-free approach," *IEEE Transactions on Robotics*, 2024.

[17] M. Hamandi, M. Tognon, and A. Franchi, "Direct acceleration feedback control of quadrotor aerial vehicles," in *2020 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2020, pp. 5335-5341.

[18] S. Rajappa, M. Ryll, H. H. Bülthoff, and A. Franchi, "Modeling, control and design optimization for a fully-actuated hexarotor aerial vehicle with tilted propellers," in *2015 IEEE international conference on robotics and automation (ICRA)*. IEEE, 2015, pp. 4006-4013.

[19] A. Franchi, R. Carli, D. Bicego, and M. Ryll, "Full-pose tracking control for aerial robotic systems with laterally bounded input force," *IEEE Transactions on Robotics*, vol. 34, no. 2, pp. 534-541, 2018.

[20] A. Franchi and A. Mallet, "Adaptive closed-loop speed control of bldc motors with applications to multi-rotor aerial vehicles," in *2017 IEEE International Conference on Robotics and Automation (ICRA)*, 2017, pp. 5203-5208.

[21] R. Verschueren, G. Frison, D. Kouzoupis, J. Frey, N. van Duijkeren, A. Zanelli, B. Novoselnik, T. Albin, R. Quirynen, and M. Diehl, "A modular open-source framework for fast embedded optimal control," *Mathematical Programming Computation*, 2021.

[22] S. Gros, M. Zanon, R. Quirynen, A. Bemporad, and M. Diehl, "From linear to nonlinear MPC: bridging the gap via the real-time iteration," *International Journal of Control*, vol. 93, no. 1, pp. 62-80, 2020.

[23] A. Bambade, S. El-Kazdadi, A. Taylor, and J. Carpentier, "Prox-qp: Yet another quadratic programming solver for robotics and beyond," in *RSS 2022-Robotics: Science and Systems*, 2022.