1

Adaptive Nonlinear MPC for Trajectory Tracking of An

Overactuated Tiltrotor Hexacopter
Yueqian Liu1 , Fengyu Quan2 and Haoyao Chen2

Abstract—Omnidirectional micro aerial vehicles (OMAVs) are

more capable of doing environmentally interactive tasks due
to their ability to exert full wrenches while maintaining stable
poses. However, OMAVs often incorporate additional actuators
and complex mechanical structures to achieve omnidirectionality.
Obtaining precise mathematical models is difficult, and the mis-
arXiv:2211.06762v1 [cs.RO] 12 Nov 2022

match between the model and the real physical system is not triv-
ial. The large model-plant mismatch significantly degrades overall
system performance if a non-adaptive model predictive controller
(MPC) is used. This work presents the L1 -MPC, an adaptive non-
linear model predictive controller for accurate 6-DOF trajectory
tracking of an overactuated tiltrotor hexacopter in the presence
of model uncertainties and external disturbances. The L1 -MPC
adopts a cascaded system architecture in which a nominal MPC
is followed and augmented by an L1 adaptive controller. The
proposed method is evaluated against the non-adaptive MPC,
the EKF-MPC, and the PID method in both numerical and
PX4 software-in-the-loop simulation with Gazebo. The L1 -MPC
reduces the tracking error by around 90% when compared to a
non-adaptive MPC, and the L1 -MPC has lower tracking errors, Fig. 1. The Omnihex tracking a 6-DOF trajectory using our customized PX4
higher uncertainty estimation rates, and less tuning requirements firmware.
over the EKF-MPC. We will make the implementations, including
the hardware-verified PX4 firmware and Gazebo plugins, open-
source at https://github.com/HITSZ-NRSL/omniHex. The Omnihex can efficiently generate a desirable wrench
envelope. However, the system is difficult to model since
complicated dynamics may arise from tilting rotors, airflow
I. INTRODUCTION turbulence, and other moving mechanical parts. Accurate
trajectory tracking ability is fundamental for completing en-
A. Background
vironmentally interactive and non-interactive tasks. Model
The past few years have seen much research and develop- predictive control proves an effective method for trajectory
ment around micro aerial vehicles (MAVs). MAVs are widely tracking tasks. However, the performance of MPC degrades
used in passive tasks, including aerial filming, rescuing, and when significant model mismatches exist. To compensate for
terrain mapping. In these tasks, no physical interaction with the model mismatch and external disturbances that may act
the environment is required. For interactive tasks such as on the system during tasks requiring physical interaction with
structure and material inspection, aerial manipulation, and wall the environment, we augment the nominal MPC with the
painting, omnidirectional MAVs have been developed as they L1 adaptive control method to achieve accurate trajectory
can generate full wrenches on their end effectors. tracking.
Different approaches for achieving omnidirectionality have
been presented [1]. One approach stands out because of its B. Related Works
optimized efficiency, where rotor tilting mechanisms are added
to a regular fixed rotor hexacopter. The tilting mechanisms The overactuated tiltrotor hexacopter with six independent
bring over-actuation and omnidirectionality to the system. So, arm tilting mechanisms is first developed in [2]. The actuator
we refer to an OMAV designed in such an approach as an effectiveness model, a least-energy control allocation method,
overactuated tiltrotor hexacopter, and we name our OMAV and a basic control design for their OMAV, Voliro, have
model Omnihex. been presented. Voliro has been studied more extensively
regarding hardware design, system modeling, PID control, and
This work was supported in part by the National Natural Science Foundation control allocation in [3]. In [4], additional propellers have been
of China under Grant U1713206 and 61673131. The corresponding author is introduced to reduce the gyroscopic effect, and rotor tilting
Haoyao Chen.
1 Yueqian Liu was with nROS-Lab. He is now with the Faculty of Aerospace mechanisms have been reworked to reduce system inertia on
Engineering, Delft University of Technology, Delft, 2629 HS, the Netherlands. their new VoliroX. Also, singularities of control allocation
Y.Liu-133@student.tudelft.nl have been studied, and corresponding handling methods have
2 Fengyu Quan and Haoyao Chen are with nROS-Lab, the School of
been developed.
Mechanical Engineering and Automation, Harbin Institute of Technology,
Shenzhen, 518055, People’s Republic of China. {18b353011@stu, Many approaches have been presented to improve the
hychen5@}.hit.edu.cn control performance of VoliroX. In [5], jerk-level LQRI has
2

been derived alongside a new control allocation algorithm • Compared to a non-adaptive MPC with nominal model
called differential allocation that can exploit the actuation dynamics and parameters, it lowers tracking error by
null space. Learning the plant-model mismatch offline using a around 90%.
Gaussian process regressor also boosts controller performance In addition, we will release an open-source simulation
[6]. However, the learned model should be updated whenever scheme and the verified PX4 firmware. These practical im-
the system parameters change due to extra payloads, changes plementations can be used for verifying other novel algo-
in battery specifications, or even repairs after damage. Fur- rithms and general Omnihex future developments, such as
thermore, another limitation is that it cannot adapt to external autonomous navigation in cluttered environments and coop-
disturbances during flights. erating with a robotic arm.
Trajectory tracking EKF-MPC has been derived for VoliroX
in [7]. EKF-MPC uses an extended Kalman filter to estimate
wrench errors that appear due to model mismatch and external II. SYSTEM MODELING
disturbances. Then the estimated wrench is fused into the
MPC formulation to achieve adaptivity. For tracking a hybrid A. Platform Overview
force and pose trajectory at the end effector in an aerial
The Omnihex is a hexacopter with its arms tilted by six
manipulation task, a hybrid NMPC has been proposed in [8].
Dynamixel servos. The servos are placed in the center base.
The hybrid NMPC is built upon the EKF-MPC in previous
Unlike the VoliroX, which has two counter-rotating rotors
work. End effector force and pose tracking is beyond the scope
per arm, the gyroscopic effect may be more evident on the
of discussion of this work, and we focus on accurate trajectory
Omnihex. Nevertheless, the gyroscopic effect is neglected in
tracking of the platform itself during free flight.
the nominal model since we use an adaptive design.
Another approach to adaptivity is to add an adaptive control
Coordinate systems are illustrated in fig. 2. We attach an
layer between the MPC and the plant. We design the layer
FRD-XYZ body frame to the geometric center of the Omnihex.
according to the L1 adaptive control theory. The L1 adaptive
For the world frame, the NED-XYZ frame is used. The
control theory provides fast adaptation while the adaptation
shown coordinate systems match the ones defined in the PX4
rate is decoupled from robustness [9]–[12]. A linear model
software.
MPC cascaded by a linear model L1 adaptive controller
for quadrotor trajectory tracking has been presented in [13].
Because linear models have been used, controller performance
B. Notations
is expected to degrade when the quadrotor is doing highly
aggressive maneuvers. The L1 adaptive control theory has Left subscripts denote coordinate frames: 𝑊 for the world
again been applied to a quadrotor by cascading a model frame and 𝐵 for the body frame. Right subscripts are abbrevi-
predictive path integral (MPPI) controller with an L1 adap- ations of value properties. Attitude quaternions are written as
tive controller in [14]. Although in [14], the advantage of 𝒒 = [𝑤, 𝑥, 𝑦, 𝑧] | , where the scalar 𝑤 is followed by the vector
augmenting a predictive controller with an L1 adaptive layer part [𝑥, 𝑦, 𝑧]. The coordinate transformation of a vector in the
is demonstrated, the computational overhead of the MPPI body frame 𝐵 𝒗 to the world frame is given in (1) that uses the
controller is often too heavy for onboard computers. With a quaternion product ⊗:
focus on the agile and highly aggressive flight of quadrotors,
work [15] presents L1 -NMPC, in which nonlinear models have 𝑊𝒗 = 𝒒 ⊗ 𝐵 𝒗 ⊗ 𝒒 −1 = rotate( 𝐵 𝒗, 𝒒) (1)
been used for both controllers. Experiments in [15] show that
tracking errors are reduced by a significant amount under large
unknown disturbances and without any parameter tuning, even
when quadrotors are flying highly agile racing trajectories.
Notable works have been done on quadrotors, but to the best
of our knowledge, L1 -MPC has not been derived for Omnihex R
yet.

C. Contribution F
D
We present the L1 -MPC for accurate trajectory tracking
of an overactuated tiltrotor hexacopter. The controller has the E
following favorable features:
• It adapts to changes in model parameters (vehicle mass,
inertia, center of mass, and propeller coefficients) and Ground Plane
time-variant external disturbances. N
• Minimal computational overhead is introduced and the D
time required for solving an optimal control problem is
reduced. Fig. 2. Illustration of the Omnihex and frames.
 3

C. Rigid Body Dynamics

We ignore the complex dynamics that result from moving
parts and airflow interference, and we model the Omnihex
as a rigid body. Rigid body dynamics consist of equations
of kinematics, as well as equations for dynamics that are R
described using the Newton-Euler equation.
The equations for kinematics are based on the definition F
of velocity and the mapping from body angular velocity to D
quaternion rate [16]. As illustrated in fig. 3 and expressed by
(2), the total wrench, 𝐵 𝒇𝑐𝑜𝑚 and 𝐵 𝝉𝑐𝑜𝑚 , that acts on the center
of gravity is used for bridging the Newton-Euler equation and
the total wrench produced by actuators, 𝐵 𝒇 𝑎 and 𝐵 𝝉𝑎 .
    Fig. 4. Propeller spinning directions and positive tilting angle configuration.
𝑚 · 𝑊 𝒗¤ 𝑚 𝒈 + rotate( 𝐵 𝒇 𝑎 , 𝒒)
= (2)
𝑱 · 𝐵 𝝎¤ + 𝐵 𝝎 × ( 𝑱 · 𝐵 𝝎) 𝐵 𝝉𝑎 − 𝐵 𝒅 𝑐𝑜𝑚 × 𝐵 𝒇 𝑎

Therefore, the complete rigid body dynamics equation is servo can change the tilt angle arbitrarily fast. While ignoring
written as: the tilt dynamics, constraints on wrench changing rates are
incorporated in the MPC design. With such assumptions,
𝑊 𝒗
 
 𝒑¤     each propeller produces forces and torques proportional to the
   1 0 
 𝒒¤  
2 𝒒 ⊗  square of the rotor angular velocity. The force 𝑓𝑖 and torque
𝐵𝝎
 =  (3)
 𝒗¤   1
 𝜏𝑖 generated by rotor number 𝑖 are:
𝑊  
 𝝎¤   𝒈 + 𝑚 rotate( 𝐵 𝒇 𝑎 , 𝒒) 

 𝐵   𝑱 −1 ( 𝝉𝑎 − 𝒅 𝑐𝑜𝑚 × 𝒇 𝑎 − 𝝎 × ( 𝑱 · 𝝎)) 
𝐵 𝐵
𝑓𝑖 = 𝜇Ω𝑖 2 , 𝜏𝑖 = 𝑘Ω𝑖 2
𝐵 𝐵 𝐵
(4)
 
where m is the mass and 𝑱 is the inertia, 𝑊 𝒗 is the vehicle
linear velocity and 𝑊 𝒗¤ is the linear acceleration, 𝒈 is the
gravitational acceleration, 𝐵 𝝎 is the body angular velocity and where 𝜇 and 𝑘 are coefficients that characterize the perfor-
¤ is the angular acceleration, 𝐵 𝒅 𝑐𝑜𝑚 is the center of mass mance of a propeller and Ω𝑖 is the angular velocity. The
𝐵𝝎
position relative to the body frame. actuator effectiveness model is described by:
 
𝐵 𝒇 𝑐𝑜𝑚 = 𝑴 [Ω21 , Ω22 , Ω23 , Ω24 , Ω25 , Ω26 ] | (5)
D. Actuator Effectiveness
𝐵 𝝉𝑐𝑜𝑚
Rotor spinning directions and the positive angle direction
is shown in fig. 4. Rotor spinning directions are configured where matrix 𝑴 is a nonlinear function of 𝜇, 𝑘, and six tilt
in such a way as to match the PX4 convention. The positive angles 𝛼1 ,· · · ,𝛼6 . 𝑴 is also referred to as the allocation matrix
angle direction for each arm is determined by the other fingers in [2] and [3], where its expression is presented in detail.
of one’s right hand, with the thumb pointing outwards from
the center. The exact configuration has been used in [2], [3].
We assume that the forces and torques produced by tilting
rotating propellers, including the gyroscopic effect, can be III. CONTROL DESIGN
compensated by adaptive measures. Also, we assume that the
A. L1 -MPC

The L1 adaptive control method is a kind of model ref-

erence adaptive control (MRAC) that features decoupled fast
adaptation and robustness. The L1 -MPC consists of two parts:
the nominal MPC and the L1 adaptive controller, where
Body Frame
the L1 adaptive controller makes the real system act as the
nominal reference model, and the nominal MPC predicts the
future system behavior based on the nominal reference model
and solves for the optimal control values. The L1 adaptive
controller is inserted between the actual plant and the nominal
Center of Mass
MPC.
As illustrated in fig. 5, the control output of the nominal
MPC acts on the combined system, where the L1 adaptive
controller takes care of the model mismatch and external
disturbances and converges the dynamics of the combined
system to the nominal dynamics. As a result, we no longer
Fig. 3. Wrench analysis of the rigid body. need to fine-tune the parameters of the MPC reference model.
4

can describe the system behavior in terms of ideal dynamics

Nominal Reference Model
𝒛ሶ = 𝓐 A and dynamics caused by the uncertainties B:
Nominal
Same 𝒛ሶ
𝒈 + 𝑚1 rotate( 𝐵 𝒇 𝑎 , 𝒒)
 
MPC
ℒ1 Adaptive Real-World System 𝒛¤ = −1
𝒖𝑚𝑝𝑐 Controller 𝒛ሶ = 𝓐 + 𝓑 𝒖ℒ1 + 𝝈 𝑱 ( 𝐵 𝝉𝑎 − 𝐵 𝒅 𝑐𝑜𝑚 × 𝐵 𝒇 𝑎 − 𝐵 𝝎 × ( 𝑱 · 𝐵 𝝎))
𝑹
     
0 𝒇 L1 𝒇Δ (10)
+ −1 ˆ𝑚
−1
𝐵 + 𝐵
Fig. 5. An overview of the L1 -MPC methodology. See (10) for symbol 𝑱 𝐵 𝒅 𝑐𝑜𝑚 𝑱 𝝉
𝐵 L1 𝝉
𝐵 Δ
definitions.
, A + B(𝒖 L1 + 𝝈)

A model predictive controller is an optimization-based where 𝑹 is the rotation matrix that corresponds to the atti-
controller that solves optimal control problems (OCP) in a tude quaternion, and 𝐵 𝒅ˆ𝑐𝑜𝑚 is the skew-symmetric matrix of
receding-horizon fashion. We formulate the OCP as follows: 𝐵 𝒅 𝑐𝑜𝑚 .
In [14] and [15], uncertainties are divided into matched
𝑁 −1
∑︁
| | | and unmatched parts. Because the actuators of a quadrotor
min (𝒆 𝑘 𝑸𝒆 𝑘 + 𝒖 𝑘 𝑹𝒖 𝑘 ) + 𝒆 𝑁 𝑸 𝑁 𝒆 𝑁 cannot generate forces on its XY plane, the corresponding
[𝒖0 ···𝒖 𝑁 −1 ]
𝑘=0
force uncertainties are called unmatched uncertainties. For


 𝒉 𝑘 ∈ [𝒉𝑙𝑏 , 𝒉𝑢𝑏 ] (6) the OMAV case, all uncertainties are matched uncertainties.

 𝒖 𝑘 ∈ [𝒖 𝑙𝑏 , 𝒖 𝑢𝑏 ]

 According to the L1 adaptive control theory, the state predictor
s.t.

 𝒙¤ = 𝑑𝑦𝑛(𝒙, 𝒖) is:
𝒛¤̂ = A + B(𝒖 L1 + 𝝈)


 𝒙 = 𝒙(𝑡)
 0 ˆ + 𝑨 𝒛˜ (11)

where where 𝒛˜ = 𝒛ˆ − 𝒛, and ∗ˆ means the estimated value of ∗. 𝝈

ˆ
 𝒑   𝒑 𝑒𝑟𝑟  is estimated by the adaptive control law, which is given in
discrete-time form as:
   
 𝒒   𝒒 𝑒𝑟𝑟 
 𝒗
𝒇¤
     
 𝒗  𝒗  𝑊 
𝒙 =  𝑊  , 𝒖 = 𝐵 𝑎 , 𝒆 = 𝑊 𝑒𝑟𝑟  , 𝒉 =  𝝎 ˆ 𝑘 = −B−1 (𝑒 𝑨𝑇𝑠 − 𝑰6 ) −1 𝑨𝑒 𝑨𝑇𝑠 𝒛˜ 𝑘
𝝈 (12)
 𝐵𝝎  𝐵 𝝉¤𝑎  𝐵 𝝎𝑒𝑟𝑟 
𝐵 
𝑭 (7)
 𝒇𝑎   𝒇𝑎 
The matrix exponential 𝑒 𝑨𝑇𝑠 refers to the discretized state
 
 𝐵   𝐵 
 𝝉𝑎   𝝉𝑎 
𝐵   𝐵   matrix that corresponds to matrix 𝑨. Matrix 𝑨 is a Hurwitz
𝑤 matrix, and it also represents the adaptive gain. Moreover,
𝒒 ⊗ 𝒒 𝑟−1𝑒 𝑓 =
𝒒 𝑒𝑟𝑟 Matrix 𝑨 is often set to a diagonal matrix with all negative
elements in practice. For an element on the diagonal line,
The state vector includes the wrench to be produced by
the bigger its absolute value is, the higher adaptive gain
actuators. With the time derivative of the actuator wrench as
it represents. With high adaptive gains, the estimation of
the control vector, we can explicitly apply constraints on the
𝝈ˆ is faster, and the system is more responsive to external
wrench changing rate and implicitly limit the tilt angle rotation
disturbances. Nevertheless, high gains tend to bring high-
speeds [7]. The position error 𝒑 𝑒𝑟𝑟 , velocity error 𝑊 𝒗 𝑒𝑟𝑟 , and
frequency oscillation to 𝝈,ˆ so to get smooth compensation,
angular velocity error 𝐵 𝝎𝑒𝑟𝑟 are the difference between the
a low pass filter is introduced:
reference value and current value. The quaternion error is
calculated using the quaternion product. Vector 𝒉 is composed 𝒖 L1 = −LPF( 𝝈)
ˆ (13)
of values constrained by an upper and a lower bound, and the
rotor thrust 𝑭 vector is obtained by the Moore–Penrose inverse Till now, we can present the complete L1 -MPC algorithm:
of a force allocation matrix 𝑩.
 
𝒇 Algorithm 1: The L1 -MPC algorithm
𝒚 , 𝑩† 𝐵 𝑎 = [𝑦 1 , · · · , 𝑦 12 ] |
𝐵 𝝉𝑎 (8) 𝒛ˆ0 ← 𝒛 0 ;
2 2 2 2 | 𝒖 𝑚 𝑝𝑐,−1 ← 0;
𝑭 = [𝑦 1 + 𝑦 2 , · · · , 𝑦 11 + 𝑦 12 ]
𝑇𝑠 ← control period;
The nonlinear model 𝒙¤ = 𝑑𝑦𝑛(𝒙, 𝒖) is fully expanded to:
while 𝑘 ≥ 0 do

 𝑊 𝒗 

 𝒖 𝑜𝑐 𝑝 ← optimal wrench rates from OCP solver;
 1 0  𝒖 𝑚 𝑝𝑐,𝑘 ← 𝒖 𝑚 𝑝𝑐,𝑘−1 + 𝒖 𝑜𝑐 𝑝 𝑇𝑠 ;
2𝒒 ⊗
 

𝐵 𝝎  update A, B, 𝒛, and 𝒛˜ 𝑘 ;
ˆ 𝑘 ← −B−1 (𝑒 𝑨𝑇𝑠 − 𝑰6 ) −1 𝑨𝑒 𝑨𝑇𝑠 𝒛˜ 𝑘 ;
 1

𝒙¤ = 
 𝒈 + 𝑚 rotate( 𝐵 𝒇 𝑎 , 𝒒) 
 (9) 𝝈 //(12)
 𝑱 −1 ( 𝝉𝑎 − 𝒅 𝑐𝑜𝑚 × 𝒇 𝑎 − 𝝎 × ( 𝑱 · 𝝎))  𝒖 L1 ,𝑘 ← −LPF( 𝝈 ˆ 𝑘 ); //(13)
𝐵 𝐵 𝐵 𝐵 𝐵
¤
 

𝐵 𝒇𝑎
 output 𝒖 L1 ,𝑘 + 𝒖 𝑚 𝑝𝑐,𝑘 to the control allocator;
𝒛¤̂ 𝑘 ← A + B(𝒖 L1 ,𝑘 + 𝝈
 

𝐵 𝝉¤𝑎
 ˆ 𝑘 ) + 𝑨 𝒛˜ 𝑘 ; //(11)
¤̂
 
𝒛ˆ 𝑘+1 ← 𝒛ˆ 𝑘 + 𝑇𝑠 𝒛 𝑘 ;
In designing the L1 adaptive controller, uncertainties only
𝑘 ← 𝑘 +1
appear as additional wrench added to the system dynamics. end
So let 𝒛¤ = [𝑊 𝒗¤ | , 𝐵 𝝎¤ | ] | and consider the uncertainties, we
 5

Note that B−1 is calculated in each loop at step four.

To reduce the computational overhead, using the property
of rotation matrices 𝑹 ∈ SO(3), the inverse operation is
transformed into:
𝑹
 −1 
𝑚 𝑹|
 
0 0
𝑚 = (14)
𝑱 −1 𝐵 𝒅ˆ𝑐𝑜𝑚 𝑱 −1 𝑚 𝐵 𝒅ˆ𝑐𝑜𝑚 𝑹 | 𝑱

Additionally, the inverse of inertia 𝑱 −1 and the constant value

in (12): (𝑒 𝑨𝑇𝑠 − 𝑰6 ) −1 can be pre-calculated at controller
initialization.

B. PID Backup Control Fig. 6. Illustration of the reference trajectory. Its XYZ and Euler Angle
representation is presented in fig. 9
Optimization-based controllers suffer from robustness and
convergence issues when the current states are too far from the
desired trajectory. No updated wrench command is available IV. EXPERIMENTS
if the OCP cannot be solved. We use a classical PID-based A. Settings
backup controller to address this issue. The controller can also
We build prototypes of the OCP solver and EKF in Python
function as a standalone controller. The algorithm is modified
using CasADi [18] and acados [19]. Then we implement the
upon the one given in the PX4 project.
controllers as optimized C++ ROS2 nodes with the generated
The position controller and attitude controller run in parallel
code. The OCP formulation’s time horizon is 1 second with
to calculate the acceleration setpoint 𝑊 𝒗¤ 𝑠 𝑝 and the angular
𝑁 = 20. The L1 -MPC and EKF-MPC run at 100 Hz. We
acceleration setpoint 𝐵 𝝎¤ 𝑠 𝑝 . The position controller consists of
select a 6-DOF trajectory (fig. 6) that is almost impossible for
an outer position loop and an inner velocity loop. The attitude
underactuated MAVs to track.
controller comprises a nonlinear quaternion-based controller
The OCP formulation for EKF-MPC is almost the same as
[17] and an inner angular velocity loop. When the PID
the one used in the L1 -MPC. The only difference is that the
controller is activated, the body wrench command is calculated
dynamics equations in (9) are modified into:
using the acceleration setpoints in (15). Otherwise, the wrench
𝒈 + 𝑚1 rotate( 𝐵 𝒇 , 𝒒)
 
command comes from the adaptive MPC.
(19)
𝑱 −1 ( 𝐵 𝝉 − 𝐵 𝒅 𝑐𝑜𝑚 × 𝐵 𝒇 − 𝐵 𝝎 × ( 𝑱 · 𝐵 𝝎))
𝐵 𝒇𝑎 = 𝑚(rotate( 𝑊 𝒗¤ 𝑠 𝑝 − 𝒈, 𝒒 −1 ) + 𝐵 𝝎 × rotate( 𝑊 𝒗, 𝒒 −1 ))
where 𝐵 𝒇 = 𝐵 𝒇 𝑎 + 𝐵 𝒇 𝐸 𝐾 𝐹 , 𝐵 𝝉 = 𝐵 𝒇 𝑎 + 𝐵 𝝉𝐸 𝐾 𝐹 , and the force
¤ 𝑠 𝑝 + 𝐵 𝒅 𝑐𝑜𝑚 × 𝐵 𝒇 𝑎 + 𝐵 𝝎 × ( 𝑱 · 𝐵 𝝎)
𝐵 𝝉𝑎 = 𝑱 · 𝐵 𝝎 an torque with the EKF subscript are estimated by the EKF.
(15)
The states 𝒙 𝐸 𝐾 𝐹 , system input 𝒖 𝐸 𝐾 𝐹 and observation
vector 𝒛 𝐸 𝐾 𝐹 of the EKF are defined as:
C. Control Allocation  𝒑 
 
 𝒒   𝒑
For this part, the results [2]–[4] are directly used. The      
control allocation algorithm returns a least-square solution
 𝒗  𝒇 𝑎
 𝒒 
𝒙𝐸 𝐾 𝐹 =  𝑊
 , 𝒖 𝐸 𝐹 𝐾 = 𝝉𝑎 , 𝒛 𝐸 𝐹 𝐾 =  𝒗  + 𝒗 𝐸 𝐾 𝐹
 𝐵  
using the pseudo-inverse matrix calculation. Measures for  𝐵 𝝎  𝐵 𝑊 
 𝒇𝐸 𝐹 𝐾   𝝎
handling kinematics singularities are implemented. 𝐵  𝐵 
 𝝉𝐸 𝐹 𝐾 
In the first step, we move the variables in the allocation 𝐵 
matrix 𝑴 to a temporary vector 𝑼: (20)
  And the system dynamics is:
𝐵 𝒇 𝑎 = 𝑴 𝑼 = 𝑴 [Ω2 𝑠 , Ω2 𝑐 , · · · , Ω2 𝑠 , Ω2 𝑐 ] | (16)
1 1 1 1 6 6 6 6
𝑊 𝒗
𝑐 𝑐
𝐵 𝝉𝑎
 
  
 1 0 
2𝒒 ⊗
 
where 𝑠𝑖 denotes sin 𝛼𝑖 and 𝑐 𝑖 denotes cos 𝛼𝑖 . To compute the 
 𝐵 𝝎 

least-square solution, we use the Moore–Penrose inverse in the 𝒙¤ 𝐸 𝐾 𝐹 =  𝒈 + 𝑚1 rotate( 𝐵 𝒇 , 𝒒)  + 𝒘𝐸𝐾 𝐹

second step:  𝑱 −1 ( 𝝉 − 𝒅 𝑐𝑜𝑚 × 𝒇 − 𝝎 × ( 𝑱 · 𝝎)) 
   𝐵 𝐵 𝐵 𝐵 𝐵 
𝒇  − 𝐵 𝝎ˆ · 𝐵 𝒇 𝐸 𝐹 𝐾 
𝑼 = 𝑴𝑐 † 𝐵 𝑎 (17)  
𝐵 𝝉𝑎

 0 

(21)
Finally, to extract individual actuator commands from 𝑼, where 𝒗 𝐸 𝐾 𝐹 is the observation noise and 𝒘 𝐸 𝐾 𝐹 is the process
we follow these equations: noise; they are both column vectors. Since the dynamics of
! disturbances are not easy to model, we assume 𝑊 𝒇¤𝐸 𝐹 𝐾 =
Ω 2𝑠
𝐵𝝎 ¤ 𝐸 𝐾 𝐹 = 0 as suggested in [7], it follows the product rule
√︃
4 2 2 −1 𝑖
Ω𝑖 = (Ω𝑖 𝑠𝑖 ) 2 + (Ω𝑖 𝑐 𝑖 ) 2 , 𝛼𝑖 = tan 𝑖
(18)
Ω2𝑖 𝑐 𝑖 for differentiation, then we get 𝐵 𝒇¤𝐸 𝐹 𝐾 = − 𝐵 𝝎ˆ · 𝐵 𝒇 𝐸 𝐹 𝐾 . To
6

estimate fast-changing time-variant disturbances, we set the

corresponding values in the process noise covariance matrix
to a relatively high value.
The EKF algorithm is presented in [20] for practical imple-
mentation. Equation (21) is discretized in the algorithm using
the Runge-Kutta 4th order method.

B. Numerical Simulation
Numerical simulation is done with the numerical integra-
tors acting as the Omnihex. We manually tweak the model
referenced in the integrator so that we can evaluate how the
L1 -MPC improves tracking accuracy. We evaluate tracking
accuracy by the position root mean square error (RMSE) and
quaternion RMSE.
We divide the experiments into four groups: A, B, C, and
D. Each group uses the same integrator model but different
controllers. The specific integrator models used are:
• A: nominal model,
• B: mass reduced by 0.5√Kg, inertia reduced by 50%, the
center of mass moved 3 cm away from the original,
• C: mass increased
√ by 0.5 Kg, inertia doubled, the center Fig. 7. Tracking RMSE reduction using the L1 -MPC.
of mass moved 3 cm away from the original,
• D: mass increased
√ by 0.5 Kg, inertia doubled, the center
of mass moved 3 cm away from the original, and extra
disturbance is applied (22, 23).
And subscripts denote the type of controllers:
• 1: the nominal MPC,
• 2: the L1 -MPC,
• 3: the EKF-MPC.
The dynamics equations for the integrator in group D are:
1
   
𝑊 𝒗¤ = 𝒈 + 𝑚 rotate(0.95( 𝐵 𝒇 𝑎 + sin( 𝐵 𝒇 𝑎 )), 𝒒) (22)
𝐵𝝎¤ 𝑱 −1 𝑻
where
𝑻 = 0.9( 𝐵 𝝉𝑎 + sin( 𝐵 𝝉𝑎 ))
− 0.95 𝐵 𝒅 𝑐𝑜𝑚 × 𝐵 𝒇 𝑎 (23)
− 𝐵 𝝎 × ( 𝑱 · 𝐵 𝝎)
Simulation results are shown in fig. 7 to fig. 10.
Fig. 7 shows the performance comparison of the L1 -
MPC and the nominal MPC on different integrator models
when tracking trajectories of different periods. As the period
decreases, the average velocity increases, and the tracking
errors increase. Nevertheless, the L1 -MPC reduces both the Fig. 8. Tracking RMSE reduction using the EKF-MPC.
position tracking RMSE and quaternion tracking RMSE by
around 90%. Fig. 8 presents the results for the EKF-MPC in
tracking after 4 seconds since the start. The direct reason is
the same fashion.
that the estimation rate of EKF is not tuned as high as the L1
As shown in fig. 8, the EKF-MPC does not perform as well
adaptive rate. With larger covariance for the process noises,
as the L1 -MPC. When the trajectory period is 15 seconds, the
the lines of EKF-MPC in fig. 10 move closer to the lines of
RMSE down percentage for position tracking is decreased to
L1 -MPC, but setting high covariance tends to introduce higher
roughly 50%, which can be explained by fig. 9 and fig. 10.
spikes and make the system more sensitive, or less robust, to
Fig. 9 shows the time-domain data of Group D with a period
noises.
equal to 15 seconds. The nominal controller completely fails in
roll and pitch tracking and exhibits a constant offset in height
tracking, while the L1 -MPC can track the trajectory on each C. Gazebo Simulation
DOF perfectly. The EKF-MPC performs in general as well In the Gazebo simulation, we compare the results of the
as the L1 -MPC; however, it only achieves offset-free height nominal MPC, the L1 -MPC, the EKF-MPC, and the PID-
 7

Fig. 9. Reference and actual trajectory (D, period: 15s).

Fig. 11. Tracking RMSE reduction of the L1 -MPC and the EKF-MPC.

Fig. 10. Eistimated wrench uncertainties (D, period: 15s).

based controller (running in PX4 offboard mode). To introduce

model mismatch to the model simulated in Gazebo, we use
a motor plugin that generates more unmodeled forces and Fig. 12. Reference trajectory and actual trajectory (period: 15s).
torques and tweaks the allocation algorithm:
√︃ √︃
Ω𝑖 = 4 (Ω2𝑖 𝑠𝑖 ) 2 + (Ω2𝑖 𝑐 𝑖 ) 2 → Ω𝑖 = 2 (Ω2𝑖 𝑠𝑖 ) 2 + (Ω2𝑖 𝑐 𝑖 ) 2 (24)
rate, resulting in larger RMSEs. Setting large covariances of
We conduct tracking experiments on trajectories of the same the process noise brings oscillation to the system and makes
shape but in different periods. The tracking errors, RMSE the system unstable. In fig. 12, On sharp turns, the EKF-MPC
reduction, and the actual trajectory are plotted in fig. 11 and cannot track the reference precisely, as seen in experiments
fig. 11. in [7]. In addition, the average time used for optimization is
Fig. 11 shows the good overall performance of the PID 1.3 microseconds (Core-i7 11800H, Ubuntu 20.04 WSL), and
controller. However, since the PID controller is entirely reac- the time needed to update the L1 controller or the EKF is
tive and does not look into the future in the time horizon, negligible.
the tracking errors increase by a large percentage as the We also notice that in the Gazebo simulation experiments,
average speed increases. The nominal MPC operates with the the handling of kinematics singularities impedes tracking. Sup-
nominal parameters calculated using the CAD file; It looks pose the maximum value of the Roll trajectory reaches 90 de-
into the future but predicts the system’s behavior based on the grees. In that case, the desired course of tilting angles becomes
inaccurate model, only to make tracking errors even larger. discontinuous due to the singularity-handling algorithms [4]
While both the L1 -MPC and the EKF-MPC improve tracking modifying the tilt angle setpoints. The singularity-handling
performance, the EKF-MPC tends to have a slower estimation algorithms introduce a threshold for the angle between the
8

body force vector and vehicle arm vectors. When the angle is R EFERENCES
within the threshold, the tilt angle rates of the corresponding [1] M. Hamandi, F. Usai, Q. Sablé, N. Staub, M. Tognon, and A. Franchi,
arms are ramped down to zero after the actual desired rates “Design of multirotor aerial vehicles: A taxonomy based on input
are calculated. As a result, the desired actuator commands are allocation,” The International Journal of Robotics Research, vol. 40,
no. 8-9, pp. 1015–1044, 2021.
not executed, thus impeding trajectory tracking. [2] O. Elkhatib, “Control allocation of a tilting rotor hexacopter,” Bachelor
Thesis, ETH Zurich, Zurich, 2017.
[3] M. Kamel, S. Verling, O. Elkhatib, C. Sprecher, P. Wulkop, Z. Taylor,
R. Siegwart, and I. Gilitschenski, “The voliro omniorientational hexa-
V. DISCUSSION copter: An agile and maneuverable tiltable-rotor aerial vehicle,” IEEE
Robotics & Automation Magazine, vol. 25, no. 4, pp. 34–44, 2018.
Since we use the PX4 SITL framework with Gazebo, the [4] K. Bodie, Z. Taylor, M. Kamel, and R. Siegwart, “Towards efficient
full pose omnidirectionality with overactuated mavs,” in Proceedings of
controller implementations can be directly deployed on an the 2018 International Symposium on Experimental Robotics, J. Xiao,
actual physical Omnihex with a few extra tunings. As the T. Kröger, and O. Khatib, Eds. Cham: Springer International Publishing,
adaptive MPCs can adapt to model mismatches, including 2020, pp. 85–95.
[5] M. Allenspach, K. Bodie, M. Brunner, L. Rinsoz, Z. Taylor, M. Kamel,
imprecise model parameters, only controller gains need to be R. Siegwart, and J. Nieto, “Design and optimal control of a tiltrotor
tuned. For the L1 -MPC, we only need to tune the adaptive micro-aerial vehicle for efficient omnidirectional flight,” The Interna-
gain matrix and the cut-off frequencies of the low pass filter. tional Journal of Robotics Research, vol. 39, no. 10-11, pp. 1305–1325,
2020.
However, for the EKF-MPC, we have two covariance matrices [6] W. Zhang, M. Brunner, L. Ott, M. Kamel, R. Siegwart, and J. Nieto,
to tune. It is harder to get a set of parameters that enable high “Learning dynamics for improving control of overactuated flying sys-
estimation rates while preserving system stability. tems,” IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 5283–
5290, 2020.
The problem of the L1 -MPC lies in its cascaded archi- [7] M. Brunner, K. Bodie, M. Kamel, M. Pantic, W. Zhang, J. Nieto, and
tecture, which allows the adaptive layer to break constraints, R. Siegwart, “Trajectory tracking nonlinear model predictive control
while the problem of the EKF-MPC is that it is hard to achieve for an overactuated mav,” in 2020 IEEE International Conference on
Robotics and Automation (ICRA), 2020, pp. 5342–5348.
fast estimation without introducing excessive oscillation. We [8] L. Peric, M. Brunner, K. Bodie, M. Tognon, and R. Siegwart, “Di-
assume that the L1 -MPC can be improved by adopting meth- rect force and pose nmpc with multiple interaction modes for aerial
ods of the EKF-MPC and vice versa. For example, we may use push-and-slide operations,” in 2021 IEEE International Conference on
Robotics and Automation (ICRA), 2021, pp. 131–137.
the L1 adaptive controller as a pure estimator to eliminate the [9] N. Hovakimyan and C. Cao, L1 Adaptive Control Theory: Guaranteed
potential of breaking constraints and integrate the estimated Robustness with Fast Adaptation. USA: Society for Industrial and
uncertainties into the OCP formulation. Also, we may add Applied Mathematics, 2010.
[10] E. Xargay, N. Hovakimyan, and C. Cao, “L1 adaptive controller for
an LPF to the EKF-MPC to decouple the estimation rate multi-input multi-output systems in the presence of nonlinear unmatched
from robustness similarly. In addition, we hypothesize that uncertainties,” in Proceedings of the 2010 American Control Conference,
integrating the control allocation and singularity handling into 2010, pp. 874–879.
[11] E. Kharisov, N. Hovakimyan, J. Wang, and C. Cao, “L1 adaptive con-
the OCP formulation will further reduce the tracking error. troller for time-varying reference systems in the presence of unmodeled
These are excellent topics for future research. nonlinear dynamics,” in Proceedings of the 2010 American Control
For the PID controller, one aspect that can be improved Conference, 2010, pp. 886–891.
[12] Z. Li and N. Hovakimyan, “L1 adaptive controller for mimo systems
is that the gains for the nonlinear quaternion-based attitude with unmatched uncertainties using modified piecewise constant adapta-
controller should be changed online according to the config- tion law,” in 2012 IEEE 51st IEEE Conference on Decision and Control
uration of arm tilt angles. Because at different configurations, (CDC), 2012, pp. 7303–7308.
[13] K. Pereida and A. P. Schoellig, “Adaptive model predictive control
the dynamics of rotation are different. for high-accuracy trajectory tracking in changing conditions,” in 2018
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), 2018, pp. 7831–7837.
[14] J. Pravitra, K. A. Ackerman, C. Cao, N. Hovakimyan, and E. A.
VI. CONCLUSION Theodorou, “L1 -adaptive mppi architecture for robust and agile control
of multirotors,” in 2020 IEEE/RSJ International Conference on Intelli-
Aimed at achieving accurate trajectory tracking on the Om- gent Robots and Systems (IROS), 2020, pp. 7661–7666.
[15] D. Hanover, P. Foehn, S. Sun, E. Kaufmann, and D. Scaramuzza,
nihex platform, whose precise model is challenging to obtain, “Performance, precision, and payloads: Adaptive nonlinear mpc for
we present the L1 -MPC, together with a backup controller quadrotors,” IEEE Robotics and Automation Letters, vol. 7, pp. 690–
plan, and the implementation of the EKF-MPC. The L1 -MPC 697, 2022.
[16] J. Diebel, “Representing attitude: Euler angles, unit quaternions, and
and EKF-MPC are both adaptive methods that can significantly rotation vectors,” Matrix, vol. 58, 01 2006.
reduce the adverse effects of imprecise model parameters, [17] D. Brescianini, M. Hehn, and R. D’Andrea, “Nonlinear quadrocopter at-
unmodeled dynamics, and external disturbances. Nevertheless, titude control. technical report,” Eidgenössische Technische Hochschule
Zürich, Zürich, Report, 2013.
the simulated experiments show that the L1 -MPC is better [18] J. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “Casadi:
regarding lower tracking errors, higher uncertainty estimation a software framework for nonlinear optimization and optimal control,”
rates, and fewer tuning efforts. Mathematical Programming Computation, vol. 11, pp. 1–36, 2019.
[19] R. Verschueren, G. Frison, D. Kouzoupis, J. Frey, N. van Duijkeren,
We look forward to deploying the proposed L1 -MPC on A. Zanelli, B. Novoselnik, T. Albin, R. Quirynen, and M. Diehl,
a real-world Omnihex. Hopefully, they are potentially helpful “acados—a modular open-source framework for fast embedded optimal
in tasks requiring accurate trajectory tracking abilities, such control,” Mathematical Programming Computation, vol. 14, pp. 147–
183, 2022.
as flying through a narrow gap, or in tasks requiring distur- [20] Q. Quan, Introduction to multicopter design and control. Springer,
bance rejection, such as flying with a robotic arm for aerial 2017.
manipulation.