An Efficient Design and Implementation of a Quadrotor Unmanned Aerial Vehicle Using Quaternion-Based Estimator

Abstract

The main goal of the research is to design a low-cost, performing quadrotor unmaned aerial vehicle (UAV) system. Because of low cost limits, the performance must be ensured by other ways. The present proposal is a quaternion-based estimator used in the control loop. In order to make the proposed solution easy to be reproduced by the reader, step-by-step instructions are given, including component choices, design, and implementation. Throughout the article, detailed description of the system model is given. The efficacy of the suggested quaternion-based predictive control is evaluated by extended experimental results.

1. Introduction

Unmanned aerial vehicles (UAV)s have fascinated many researchers and engineers, as they turned out to be accessible in a large variety of applications, not just for costly military operations. Nowadays, UAVs have a broad range of applications, such as: image capturing, aerial recording, military operations, operations in hard-to-reach areas, etc., [1,2,3,4,5,6,7]. Along with the development of wireless communications, the control of UAVs has become extremely precise, robust, and even predictive. New research results in the design of UAVs and new application areas include advanced and complex control techniques like robust and adaptive control, algorithms for different flight conditions, fault tolerance, disturbance rejection, etc., [8,9,10,11,12,13]. All these methods increase the complexity and the cost of the UAV. Because of the extremely alert technological progress registered in the past two decades, the global industrialization and the minimization of the costs of electronic components, countless researchers have shown a high interest in the development of various devices helpful for the society.

One key issue regarding the control of UAVs resides in the estimation of their position. Various methods have been proposed. An efficient method is presented in [14], both from the point of view of the algorithm’s performance and from the point of view of using the processing capacity of a microcontroller. Several estimation algorithms are compared in [15], with the results showing that the extended Kalman algorithm is slower in terms of processing time than Madgwick algorithm [15].

The approach of estimating the pitch and roll coordinates presented in [16] constitutes a reference that fits perfectly in the context of the present paper. For the application of the algorithm proposed in the paper, a method of fusion of the data received from an accelerometer, a gyroscope, and a magnetometer was used to estimate, accurately, the position of a flight apparatus. A combination of the extension of the classical Kalman filtering algorithm and the sequential geometric correction is proposed, completely eliminating the magnetic distortions captured by the sensor. In addition, the paper offers a clear and concise comparison between certain popular approaches to the problem of estimating the coordinates of a flight apparatus and the method proposed by the author. Both the improvements and the problems that arise in the implementation of this method are presented.

From all the knowledge resulting from this state-of-art, it can be concluded that UAVs can be made at a relatively lower cost. Simple transducers can be used, as long as this is compensated by a high performance and optimized estimation algorithm. The authors already designed a cheap and easy to use two-rotor equipment, in order to be multiplied for laboratory works [17,18].

Quaternion framework is widely used today to avoid locks and to ensure better computational efficiency [19,20]. The field of application is large, ranging from mechanical systems [21] and medical robots [22] to neural networks [23] and human activities and postures recognition [24], all research papers reporting remarkable results. Quaternions are also used in UAV control with great success. In [25], the authors developed a nonlinear state space model using the quaternion and angular velocity as state variables, which simplifies the system dynamics. The main focus of the research is directed toward the feedback linearization of the model. The simulation results are presented solely for the attitude stabilization task of the quadcopter. A quaternion representation of the attitude of a quadrotor is also used in [26], where various control methods are discussed and compared, such as the PD, LQR, and backstepping methods. Various case scenarios are discussed including noisy data, actuator restrictions, external disturbances. The attitude control of a quadrotor is designed in a quaternion framework in [27], to avoid gimbal lock and for better computational efficiency. The controllers are tuned based on third-order sliding mode control, with a low-pass filter to reduce chattering and a disturbance observer to cover disturbance estimation problems. To ensure the robustness, a disturbance compensation term is also included in the control law. The simulation results show that the proposed method is efficient. In [28], two variants of adaptive state space controllers for attitude stabilization and self-tuning of a quadrotor are proposed. The effectiveness of the approach is demonstrated through simulations that use a quaternion-based nonlinear dynamic model of a quadrotor. A quaternion representation of the attitude of a quadrotor is also used in [29], where a quaternion-energy-based control law is defined as a Lyapunov function, with the control laws described with unit quaternions and their axis-angle representation. Various simulation and experimental results are presented. Unit quaternions are also used in [30] to describe a simple yet complete dynamic model for the rotational and translational dynamics of unmanned aerial vehicles, whereas dual quaternions are explained and used for robotic systems with multiple rotations and translations. An unmanned aerial vehicle described with unit quaternions is presented in [31]. In this case, a quaternion-passivity-based control is derived. The experimental results and numerical simulations validate the results. Intermediary quaternions are used in the design of a backstepping control technique with integral properties in [32]. Compared to classical quaternions, the proposed approach has also the advantage that one specific orientation corresponds to only one intermediary quaternion, which helps coping with the unwinding phenomenon. Numerical simulations, as well as experimental tests, are presented. The robustness of the algorithm is also tested during the numerical simulations only. In [33], a quaternion-based guidance law is proposed which feeds into an attitude control system based on a PD+ control law. A quaternion control scheme for a quadrotor is also proposed in [34]. An attitude control algorithm is developed to stabilize the vehicle’s heading and an additional position control law for stabilization of the vehicle in all states. In this case also, numerical and experimental results are presented to validate the approach. An advanced control scheme, also based on quaternions, is presented in [35] for the attitude control of a quadrotor. Here, both the model and the proportional squared control algorithm are implemented in the quaternion space. Extended simulation results are included to demonstrate the efficacy of the suggested novel approach. Quaternions for attitude control are also used in [36], where a quaternion multiplicative formula is proposed to obtain the change of the attitude angle of a quadrotor. Only some practical solutions are presented.

Other recent significant results in UAV control includes more complex structures or calculus. In [37], a control structure based on a hierarchical scheme is proposed, consisting of an energy-based control to stabilize the vehicle translational dynamics and to attenuate the payload oscillation and a nonlinear state feedback controller based on a linear matrix inequality (LMI) to control the quadrotor rotational dynamics. The authors of [38] propose a neuroadaptive integral robust controller, while [39] discusses the dynamic motion planning and control of an UAV using Direct and the Second Method of Lyapunov. An interesting, but complex solution is proposed in [40], where the dynamic system is divided in two subsystems driven by the translational and the rotational dynamics, based on a linear parameter-varying model.

All these approaches have in common the use of advanced control algorithms, with the major drawback of requiring expensive hardware for implementation purposes. Thus, the main objective of the present work is to design and implement a low-cost, easy to use quadrotor UAV, accessible for any user. A quaternion-based estimator is proposed, similarly to existing research studies. However, in terms of the proposed control strategy, the classical PID controller is used, instead of advanced control algorithms. In this way, the implementation of the control strategy is simplified, which triggers the possibility of using low-cost devices for measurement and control. The final control structure includes four controllers, one for each direction of movement. Step by step design and implementation details are presented in order to be easily reproduced by the reader. Using simulation and experimental data, the proposed method is validated. The results show that similar closed loop performance can be achieved using our proposed approach, compared to other more advanced control strategies. The major advantage is that using our proposed method, these results are achieved using a low-cost UAV with a simple, yet efficient control strategy. The novelty of this work consists, thus, in a quaternion-based estimator and classical Proportional-Integral-Derivative (PID) control strategy, implemented using low-cost microcontroller and sensors. For the proposed remote control, performances are imposed in terms of rejecting a moderate range of disturbances and filtering sensor noisy signals.

The rest of this paper is organized as follows. The materials and methods used are presented in the next section. The resulting quadrotor UAV prototype, along with experimental data, is detailed in Section 3. Finally, conclusions are presented in Section 4.

2. Materials and Methods

From construction point of view, the system includes the following elements: plastic skeleton for the flight apparatus; support for the electrical circuit of the remote control; electrical circuits; ATmega32U4 and ATmega 328 microcontrollers; four DC motors; four electronic velocity controllers; four propellers; two wireless remote communication modules and a position detection module.

The main aspects of the flight apparatus described in this work are defined by: the number of engines, the position of the support arms, the mass and the center of gravity of the whole assembly. The arms are mounted in “X,” to allow easy change of direction, and the center of gravity is fixed at the intersection of the axes of the arms. The change of direction is facilitated by the control of the angular velocity of the engines. The motors are positioned as follows: two motors on one diagonal are rotated in the same direction, while the remaining two motors on the other diagonal rotate in the opposite direction. Viewed as a whole, the system is composed of a four-arm flight apparatus mounted in “X” and a remote control that provides references to the control circuit located on the quadrotor. They communicate via the UART protocol, using two RF transmission and reception modules. Two-way data exchanges are made between the quadrotor and the remote control, so both items send data and await receipt.

Regarding the mechanical design of the system, a variety of computer-aided design environments could be used to create 3D drawings and model the parts necessary for the physical realization of the system. In the present work AutoCAD and SolidWorks were adopted. In addition, Ultimaker Cura–a G code generator and a 3D printer that could correctly interpret the generated code—was operated to create the remote control.

After choosing the components, measuring their dimensions and making the connections, an electrical scheme could be conceived. The present practice used a CAD/CAM environment provided by Autodesk, called Eagle.

The device is designed in such way that the center of gravity coincides with the geometric one, also serving as the center of the coordinate system attached to the quadrotor. This coordinate system describes the relative movements of the flight apparatus to a fixed coordinate system, with an axis perpendicular to the earth’s surface. The other two axes of the fixed coordinate system can be chosen so as to coincide with cardinal points whose axes are perpendicular (for example north-east or south-west).

Like any aerial vehicle, this system has also six degrees of freedom, meaning three movements of translation and three of rotation. All these movement possibilities are strongly dependent on the velocity and implicitly the angular velocity of the four engines. Therefore, depending on these aspects, the following kinetic forces and moments developed and applied to the quadrotor can be distinguished: the altitude advance, the gyroscopic effect, the yaw moment, the pitch moment, the roll moment and, of course, the force of gravitational attraction. The increase or decrease of altitude is possible by simultaneously increasing or decreasing the velocity of all engines. In order to maintain a constant altitude it is necessary to drive the engines at the same velocity, each developing the same angular velocity. Unlike the altitude movement, the kinetic yaw, pitch, and roll momenta are obtained by differentiating the engine velocity. The yaw moment, or rotation around the vertical axis, is obtained by simultaneously increasing the velocity of two motors rotating in the same direction. Depending on the chosen engine group, the flight apparatus will rotate clockwise or trigonometrically.

2.1. Quaternion-Based Estimator

In order to obtain the orientation angles and to facilitate the calculus, two representations can be used, namely: Euler angles and quaternions.

Quaternions are used to express the orientation of a coordinate system to a reference system [41]. Given an angle of rotation Ψ about the axis ȓ, an orientation of the coordinate system B can be represented with respect to the system A as follows [41]:

$${}_{\mathrm{B}}{}^{\mathrm{A}}\widehat{\mathrm{q}}=\left[{\mathrm{q}}_1{\mathrm{q}}_2{\mathrm{q}}_3{\mathrm{q}}_{4 }\right]= \left[\cos \frac{\mathsf{\Psi }}{2}-{\mathrm{ȓ}}_{\mathrm{x}}\sin \frac{\mathsf{\Psi }}{2}-{\mathrm{ȓ}}_{\mathrm{y}}\sin \frac{\mathsf{\Psi }}{2}-{\mathrm{ȓ}}_{\mathrm{z}}\sin \frac{\mathsf{\Psi }}{2}\right]$$

(1)

The terms ${\mathrm{ȓ}}_{\mathrm{x}}$, ${\mathrm{ȓ}}_{\mathrm{y}}$,${\mathrm{ȓ}}_{\mathrm{z}}$ represent the components of the unity vector ȓ of the reference system A.

A very important advantage presented by this angle expression method is that the product of two quaternions ${}_{\mathrm{D}}{}^{\mathrm{C}}\widehat{\mathrm{q}}$ and ${}_{\mathrm{E}}{}^{\mathrm{D}}\widehat{\mathrm{q}}$ represents the orientation of the system E with respect to the reference system C.

Moreover, the orientation described by a quaternion ${}_{\mathrm{B}}{}^{\mathrm{A}}\widehat{\mathrm{q}}=\left[{\mathrm{q}}_{1 }{\mathrm{q}}_{2 }{\mathrm{q}}_{3 }{\mathrm{q}}_4\right]$ can be expressed by the rotation matrix ${}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}$, representing the rotation of the coordinate system B with respect to the reference system A. The dependence between the quaternion terms and the rotation matrix is presented in Equation (2) [16,20].

$${}_{\mathrm{B}}{}^{\mathrm{A}}\mathrm{R}=\left[\begin{array}{ccccc}2{\mathrm{q}}_1^2+2{\mathrm{q}}_2^2-1& \hspace{1em}& 2({\mathrm{q}}_1{\mathrm{q}}_4+{\mathrm{q}}_2{\mathrm{q}}_3)& \hspace{1em}& 2({\mathrm{q}}_2{\mathrm{q}}_4-{\mathrm{q}}_1{\mathrm{q}}_3)\\ 2({\mathrm{q}}_2{\mathrm{q}}_3-{\mathrm{q}}_1{\mathrm{q}}_4)& \hspace{1em}& 2{\mathrm{q}}_1^2+2{\mathrm{q}}_3^2-1& \hspace{1em}& 2({\mathrm{q}}_1{\mathrm{q}}_2+{\mathrm{q}}_3{\mathrm{q}}_4)\\ 2({\mathrm{q}}_1{\mathrm{q}}_3+{\mathrm{q}}_2{\mathrm{q}}_4)& \hspace{1em}& 2({\mathrm{q}}_3{\mathrm{q}}_4-{\mathrm{q}}_1{\mathrm{q}}_2)& \hspace{1em}& 2{\mathrm{q}}_1^2+2{\mathrm{q}}_2^4-1\end{array}\right]$$

(2)

Although, from a computational point of view, obtaining orientation using quaternions is more efficient, they are hard to interpret physically. Thus, in order to have a clear picture of the real movement, the orientations expressed by quaternions are transformed into representations using Euler angles. To carry out these transformations, Equations (3)–(5) could be used [21].

$$\mathsf{\Psi }=\mathrm{atan}\left(2{\mathrm{q}}_2{\mathrm{q}}_3-2{\mathrm{q}}_1{\mathrm{q}}_4, 2{\mathrm{q}}_1^2+2{\mathrm{q}}_2^2-1\right)$$

(3)

$$\mathsf{\theta }=\arcsin \left(2{\mathrm{q}}_1{\mathrm{q}}_3+2{\mathrm{q}}_2{\mathrm{q}}_4\right)$$

(4)

$$\mathsf{\Phi }=\mathrm{atan}\left(2{\mathrm{q}}_3{\mathrm{q}}_4-2{\mathrm{q}}_1{\mathrm{q}}_2, 2{\mathrm{q}}_1^2+2{\mathrm{q}}_4^2-1\right)$$

(5)

In order to obtain the real values of the angles, a sensor with 9 degrees of freedom was used, consisting of an accelerometer, a gyroscope, and a magnetometer. The sensor used is from the MPU9250 family and communicates with the microcontroller via the I2C interface, at a frequency of 400 kHz. The I2C protocol is a very popular data transmission protocol, due to the multitude of advantages it presents [42]. For data filtering and estimating the orientation of the aerial vehicle, the quaternion representation described above was used. With a physical interpretation much closer to reality, the data provided by the gyroscope are filtered and estimated easily. Thus, the angular positions on the X, Y, and Z axes are arranged in a vector W as described in Equation (6). In addition to these three elements, on the first position in the vector is inserted the term 0 in order to be able to perform quaternion products.

$$\mathrm{W}= \left[0{ \mathrm{w}}_{\mathrm{x} }{\mathrm{w}}_{\mathrm{y} }{\mathrm{w}}_{\mathrm{z} }\right]$$

(6)

$${}_{\mathrm{Sensor}}{}^{\mathrm{Ref}}\dot{\mathrm{q}}= {}_{\mathrm{Sensor}}{}^{\mathrm{Ref}}\mathrm{q}\otimes \mathrm{W}$$

(7)

With the angular position arranged in the vector W it is possible to compute the orientation change of the coordinate system given by the earth to the coordinate system attached to the UAV. This calculus is represented in Equation (7).

Where the term ${}_{\mathrm{Sensor}}{}^{\mathrm{Ref}}\mathrm{q}$ represents the current orientation of the coordinate system given by the earth to the quadrotor coordinate system.

In order to obtain an orientation of the coordinate system attached to the quadrotor with respect to the reference one, at a time t it is necessary to perform the mathematical operations detailed in Equations (8) and (9).

$${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\dot{\mathrm{q}}{}_{\mathrm{gyro},\mathrm{k}}=\frac{1}{2} {}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{est}, \mathrm{k}-1}\otimes \mathrm{W}$$

(8)

$${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{gyro},\mathrm{k}}={}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{est}, \mathrm{k}-1}+ {}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\dot{\mathrm{q}}{}_{\mathrm{gyro},\mathrm{k}}{\mathrm{T}}_{\mathrm{s}}$$

(9)

where T_S represents the sampling time, and $\mathrm{t}=\mathrm{k}·{\mathrm{T}}_{\mathrm{S}}$.

Because of the nature of the data from the accelerometer, an optimization problem can be formulated in which the orientation of the sensor ${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}$and, implicitly of the flight system, is given by minimizing the difference between the orientation of the reference system of the earth ${\mathrm{d}}_{\mathrm{ref}}$ and that of the sensor, ${\mathrm{d}}_{\mathrm{sensor}}$. The objective function to be minimized of is described by Equations (10) and (11), with the components detailed in (12)–(14).

$$\mathrm{o}\mathrm{f}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{d}}_{\mathrm{ref}},{ \mathrm{d}}_{\mathrm{sensor}}\right)={}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}^{\ast }\otimes { \mathrm{d}}_{\mathrm{ref}}\otimes {}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}-{ \mathrm{d}}_{\mathrm{sensor}}$$

(10)

$$\begin{gathered} \mathrm{o}\mathrm{f}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{d}}_{\mathrm{ref}},{ \mathrm{d}}_{\mathrm{sensor}}\right)= \\ \left[\begin{array}{c}2{\mathrm{d}}_{\mathrm{rx}}\left(\frac{1}{2}-{\mathrm{q}}_3^2-{\mathrm{q}}_4^2)+2{\mathrm{d}}_{\mathrm{ry}}({\mathrm{q}}_1{\mathrm{q}}_4+{\mathrm{q}}_2{\mathrm{q}}_3)+2{\mathrm{d}}_{\mathrm{rz}}({\mathrm{q}}_2{\mathrm{q}}_4-{\mathrm{q}}_1{\mathrm{q}}_3\right)-{\mathrm{d}}_{\mathrm{sx}}\\ 2{\mathrm{d}}_{\mathrm{rx}}({\mathrm{q}}_2{\mathrm{q}}_3-{\mathrm{q}}_1{\mathrm{q}}_4)+2{\mathrm{d}}_{\mathrm{ry}}(\frac{1}{2}-{\mathrm{q}}_2^2-{\mathrm{q}}_4^2)+2{\mathrm{d}}_{\mathrm{rz}}({\mathrm{q}}_1{\mathrm{q}}_2+{\mathrm{q}}_3{\mathrm{q}}_4)-{\mathrm{d}}_{\mathrm{sy}}\\ 2{\mathrm{d}}_{\mathrm{rx}}({\mathrm{q}}_1{\mathrm{q}}_3+{\mathrm{q}}_2{\mathrm{q}}_4)+2{\mathrm{d}}_{\mathrm{ry}}({\mathrm{q}}_3{\mathrm{q}}_4-{\mathrm{q}}_1{\mathrm{q}}_2)+2{\mathrm{d}}_{\mathrm{rz}}(\frac{1}{2}-{\mathrm{q}}_2^2-{\mathrm{q}}_3^2)-{\mathrm{d}}_{\mathrm{sz}}\end{array}\right] \end{gathered}$$

(11)

$${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}= \left[{\mathrm{q}}_1 {\mathrm{q}}_2 {\mathrm{q}}_3 {\mathrm{q}}_4\right]$$

(12)

$${\mathrm{d}}_{\mathrm{ref}}= \left[0_{ }{\mathrm{d}}_{\mathrm{rx} }{\mathrm{d}}_{\mathrm{ry} }{\mathrm{d}}_{\mathrm{rz}}\right]$$

(13)

$${\mathrm{d}}_{\mathrm{sensor}}= \left[0_{ }{\mathrm{d}}_{\mathrm{sx} }{\mathrm{d}}_{\mathrm{sy} }{\mathrm{d}}_{\mathrm{sz}}\right]$$

(14)

In order to solve this optimization problem, the conjugate gradient method is used, a simple, efficient method that requires a relatively low computing power [43]. However, the conjugate gradient method presents a number of disadvantages related to the algorithm step, μ and the initial point ${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_0$. Equations (15) and (16) describe the estimation of future orientation ${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{k}+1}$.

$${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{k}+1}= {}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{k}}-\mathsf{\mu }\frac{\mathrm{F}}{\Vert \mathrm{F}\Vert }$$

(15)

$$\mathrm{F}=\frac{\partial \mathrm{f}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{d}}_{\mathrm{ref}},{ \mathrm{d}}_{\mathrm{sensor}}\right)}{\partial \left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{d}}_{\mathrm{ref}}\right)}\mathrm{o}\mathrm{f}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{d}}_{\mathrm{ref}},{ \mathrm{d}}_{\mathrm{sensor}}\right)$$

(16)

The general cost function of given in (10) can be simplified to be easy to implement even in a low-cost microcontroller. Because of the fact that by convention gravitational acceleration determines only the Z axis of the reference system, this objective function can be expressed as in Equation (17), while the vectors ${\mathrm{d}}_{\mathrm{ref}}$ and ${\mathrm{d}}_{\mathrm{sensor}}$ are given in (18) and (19).

$$\mathrm{o}\mathrm{f}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{d}}_{\mathrm{ref}},{ \mathrm{d}}_{\mathrm{sensor}}\right)= \left[\begin{array}{c}2\left({\mathrm{q}}_2{\mathrm{q}}_4-{\mathrm{q}}_1{\mathrm{q}}_3\right)-{\mathrm{d}}_{\mathrm{x}} \\ 2\left({\mathrm{q}}_1{\mathrm{q}}_2+{\mathrm{q}}_3{\mathrm{q}}_4\right)-{\mathrm{d}}_{\mathrm{y}}\\ 2\left(\frac{1}{2}-{ \mathrm{q}}_2^2-{\mathrm{q}}_3^2\right)-{\mathrm{d}}_{\mathrm{z}}\end{array}\right]$$

(17)

$${\mathrm{d}}_{\mathrm{ref}}= \left[0_{ }{0}_{ }{0}_{ }1\right]$$

(18)

$${\mathrm{d}}_{\mathrm{sensor}}= \left[0_{ }{\mathrm{d}}_{\mathrm{x} }{\mathrm{d}}_{\mathrm{y} }{\mathrm{d}}_{\mathrm{z} }\right]$$

(19)

The data obtained from the magnetometer will be processed in the same way as the data obtained from the accelerometer, but with a more laborious processing given by the decomposition of the earth’s magnetic field in both a component on the X axis and one on the Z axis. To obtain the next orientations ${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{k}+1}$, the same conjugate gradient algorithm will be used. Equation (20) describes the objective function, with the terms detailed in (21) and (22), while Equation (23) presents the gradient of the objective function.

$$\begin{gathered} \mathrm{o}\mathrm{f}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{m}}_{\mathrm{ref}},{ \mathrm{m}}_{\mathrm{sensor}}\right)= \\ \left[\begin{array}{c}2{\mathrm{m}}_{\mathrm{rx}}\left(\frac{1}{2}-{\mathrm{q}}_3^2-{\mathrm{q}}_4^2)+2{\mathrm{m}}_{\mathrm{rz}}({\mathrm{q}}_2{\mathrm{q}}_4-{\mathrm{q}}_1{\mathrm{q}}_3\right)-{\mathrm{m}}_{\mathrm{sx}}\\ 2{\mathrm{m}}_{\mathrm{rx}}({\mathrm{q}}_2{\mathrm{q}}_3-{\mathrm{q}}_1{\mathrm{q}}_4)+2{\mathrm{m}}_{\mathrm{rz}}({\mathrm{q}}_1{\mathrm{q}}_2+{\mathrm{q}}_3{\mathrm{q}}_4)-{\mathrm{m}}_{\mathrm{sy}}\\ 2{\mathrm{m}}_{\mathrm{rx}}({\mathrm{q}}_1{\mathrm{q}}_3+{\mathrm{q}}_2{\mathrm{q}}_4)+2{\mathrm{m}}_{\mathrm{rz}}(\frac{1}{2}-{\mathrm{q}}_2^2-{\mathrm{q}}_3^2)-{\mathrm{m}}_{\mathrm{sz}}\end{array}\right] \end{gathered}$$

(20)

$${ \mathrm{m}}_{\mathrm{ref}}= \left[0_{ }{\mathrm{m}}_{\mathrm{rx} }{0}_{}{\mathrm{m}}_{\mathrm{rz}}\right]$$

(21)

$${ \mathrm{m}}_{\mathrm{sensor}}= \left[0_{ }{\mathrm{m}}_{\mathrm{sx} }{\mathrm{m}}_{\mathrm{sy} }{\mathrm{m}}_{\mathrm{sz}}\right]$$

(22)

$$\mathrm{F}=\frac{\partial \mathrm{of}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{m}}_{\mathrm{ref}},{ \mathrm{m}}_{\mathrm{sensor}}\right)}{\partial \left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{m}}_{\mathrm{ref}}\right)}\mathrm{o}\mathrm{f}\left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}},{ \mathrm{m}}_{\mathrm{ref}},{ \mathrm{m}}_{\mathrm{sensor}}\right)$$

(23)

In order to obtain both a measurement and an accurate estimation of the orientation of the quadrotor, it is necessary to compose the two objective functions presented in Equations (10) (or the simplified form in (17)) and (20). Also, the gradient of both functions will be used to implement the conjugate gradient algorithm for the combination of functions. The composition will be noted with ${\mathrm{f}}_{\mathrm{com}}$ and the gradient of this compound function will be noted by ${\mathrm{F}}_{\mathrm{com}}$. In addition, to make the algorithm more efficient, the step μ will be variable and recomputed at each iteration, as shown in Equation (24). The algorithm and the gradient of the new objective function are presented in Equations (25) and (26):

$${\mathsf{\mu }}_{\mathrm{t}}= \mathsf{\alpha }\Vert {}_{\mathrm{Ref}}{}^{\mathrm{Actual}}\dot{\mathrm{q}}{}_{\mathrm{gyro},\mathrm{k}}\Vert {\mathrm{T}}_{\mathrm{S}}$$

(24)

$${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{com},\mathrm{k}}={}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}-1}-{\mathsf{\mu }}_{\mathrm{t}}\frac{{\mathrm{F}}_{\mathrm{com}}}{\Vert {\mathrm{F}}_{\mathrm{com}}\Vert }$$

(25)

$${\mathrm{F}}_{\mathrm{com}}=\left[\begin{array}{c}\frac{\partial \mathrm{f}({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}-1}, {\mathrm{d}}_{\mathrm{Sensor}})}{\partial \left({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}-1}\right)}\mathrm{f}({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}-1}, {\mathrm{d}}_{\mathrm{Sensor}})\\ \frac{\partial \mathrm{f}({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}-1}, {\mathrm{m}}_{\mathrm{ref}}, {\mathrm{m}}_{\mathrm{Sensor}})}{\partial ({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}-1}, {\mathrm{m}}_{\mathrm{ref}})}\mathrm{f}({}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}-1}, {\mathrm{m}}_{\mathrm{ref}}, {\mathrm{m}}_{\mathrm{Sensor}})\end{array}\right]$$

(26)

where $\mathsf{\alpha }$ is a constant chosen experimentally to minimize the measurements noise from the accelerometer and magnetometer, ${\mathrm{T}}_{\mathrm{S}}$ is the sampling period, ${}_{\mathrm{Ref}}{}^{\mathrm{Actual}}\dot{\mathrm{q}}{}_{\mathrm{gyro},\mathrm{k}}$, represents the orientation given by the gyroscope, computed using Equation (8).

Because of the fusion of measurements from the gyroscope, ${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{gyro},\mathrm{k}}$ and those from the accelerometer and magnetometer ${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{com},\mathrm{k}}$, a weighted, very accurate estimate is obtained, as presented in Equation (27). The weight ${\mathrm{P}}_{\mathrm{k}}$ will be computed at each iteration based on the step ${\mathsf{\mu }}_{\mathrm{t}}$, a control constant $\mathsf{\beta }$, and the sampling period ${\mathrm{T}}_{\mathrm{S}}$, as in (28).

$${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}}={ \mathrm{P}}_{\mathrm{k}}{}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{com},\mathrm{k}}+\left(1-{\mathrm{P}}_{\mathrm{k}}\right){}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{gyro},\mathrm{k}}$$

(27)

$${\mathrm{P}}_{\mathrm{k}}= \frac{\mathsf{\beta }}{\frac{{\mathsf{\mu }}_{\mathrm{t}}}{{\mathrm{T}}_{\mathrm{E}}}+\mathsf{\beta }}$$

(28)

The proposed filter in (27) and (28) ensures an accurate estimation such that ${}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\widehat{\mathrm{q}}{}_{\mathrm{est},\mathrm{k}}\to {}_{\mathrm{Ref}}{}^{\mathrm{Sensor}}\mathrm{q}{}_{\mathrm{k}}$ as k→∞. This can be easily proved using the classical Lyapunov function.

At each iteration, after obtaining the current estimate, Equations (3)–(5) are used to express the Euler angle orientation, which gives a much easier to understand perspective on the movement of the quadrotor.

After obtaining the orientation angles and converting them from quaternions to Euler angles, at each iteration the rotation matrices ${\mathrm{R}}_{\mathrm{x}}\left(\mathsf{\Phi }\right)$, ${\mathrm{R}}_{\mathrm{y}}\left(\mathsf{\theta }\right)$, and ${\mathrm{R}}_{\mathrm{z}}\left(\mathsf{\Psi }\right)$ will be constructed. With these matrices, the rotation matrix of the entire system ${\mathrm{R}}_{\mathrm{x}\mathrm{y}\mathrm{z}}(\mathsf{\Psi },\mathsf{\theta },\mathsf{\Phi })$ is computed, as described in Equations (29)–(32).

$${\mathrm{R}}_{\mathrm{x}}\left(\mathsf{\Phi }\right)= \left[\begin{array}{ccc}1& 0& 0\\ 0& \mathrm{c}\left(\mathsf{\Phi }\right)& -\mathrm{s}\left(\mathsf{\Phi }\right)\\ 0& \mathrm{s}\left(\mathsf{\Phi }\right)& \mathrm{c}\left(\mathsf{\Phi }\right)\end{array}\right]$$

(29)

$${\mathrm{R}}_{\mathrm{y}}\left(\mathsf{\theta }\right)= \left[\begin{array}{ccc}\mathrm{c}\left(\mathsf{\theta }\right)& 0& \mathrm{s}\left(\mathsf{\theta }\right)\\ 0& 1& 0\\ -\mathrm{s}\left(\mathsf{\theta }\right)& 0& \mathrm{c}\left(\mathsf{\theta }\right)\end{array}\right]$$

(30)

$${\mathrm{R}}_{\mathrm{z}}\left(\mathsf{\Psi }\right)= \left[\begin{array}{ccc}\mathrm{c}\left(\mathsf{\Psi }\right)& -\mathrm{s}\left(\mathsf{\Psi }\right)& 0\\ \mathrm{s}\left(\mathsf{\Psi }\right)& \mathrm{c}\left(\mathsf{\Psi }\right)& 0\\ 0& 0& 1\end{array}\right]$$

(31)

$$\begin{gathered} {\mathrm{R}}_{\mathrm{xyz}}\left(\mathsf{\Phi },\mathsf{\theta },\mathsf{\Psi }\right)={\mathrm{R}}_{\mathrm{x}}\left(\mathsf{\Psi }\right)·{\mathrm{R}}_{\mathrm{y}}\left(\mathsf{\theta }\right)·{\mathrm{R}}_{\mathrm{z}}\left(\mathsf{\Phi }\right)= \\ \left[\begin{array}{ccc}\mathrm{c}\left(\mathsf{\theta }\right)\mathrm{c}\left(\mathsf{\Psi }\right)& \mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\mathrm{c}\left(\mathsf{\Psi }\right)-\mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\Psi }\right)& \mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\mathrm{c}\left(\mathsf{\Psi }\right)+ \mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\Psi }\right)\\ \mathrm{c}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Psi }\right)& \mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Psi }\right)+\mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\Psi }\right)& \mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Psi }\right)-\mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\Psi }\right)\\ -\mathrm{s}\left(\mathsf{\theta }\right)& \mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\theta }\right)& \mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\theta }\right)\end{array}\right] \end{gathered}$$

(32)

where $\mathrm{c}\left(\mathsf{\Psi }\right)=\cos \left(\mathsf{\Psi }\right)$, $\mathrm{s}\left(\mathsf{\Psi }\right)=\sin \left(\mathsf{\Psi }\right)$, $\mathrm{c}\left(\mathsf{\theta }\right)=\cos \left(\mathsf{\theta }\right)$, $\mathrm{s}\left(\mathsf{\theta }\right)=\sin \left(\mathsf{\theta }\right)$, $\mathrm{c}\left(\mathsf{\Phi }\right)=\cos \left(\mathsf{\Phi }\right)$, $\mathrm{s}\left(\mathsf{\Phi }\right)= \sin \left(\mathsf{\Phi }\right)$.

2.2. Quadrotor Kinematic and Dynamic Model

In order to establish an efficient mathematical model, as close as possible to the reality, which ensures greater system controllability, it is necessary to use the Equations of Newton classical mechanics and of Euler for angular motions. It is also necessary to take into account both the relative movements of the fixed coordinate system (in this case, the earth), as well as the relative dynamics of the coordinate system attached to the quadrotor. Thus, two vectors, ${\mathrm{P}}_{\mathrm{p}}$ and ${\mathrm{P}}_{\mathrm{a}}$, will be used, described by Equations (33) and (34). ${\mathrm{P}}_{\mathrm{p}}$ is the vector of the linear and angular positions of the flight system relative to earth, while ${\mathrm{P}}_{\mathrm{a}}$ is the vector of the linear and angular velocities of the quadrotor.

$${\mathrm{P}}_{\mathrm{p}}= {\left[\begin{array}{cccccc}\mathrm{x}& \mathrm{y}& \mathrm{z}& \mathsf{\Phi }& \mathsf{\theta }& \mathsf{\Psi }\end{array}\right]}^{\mathrm{T}}$$

(33)

$${\mathrm{P}}_{\mathrm{a}}= {\left[\begin{array}{cccccc}\mathrm{u}& \mathrm{v}& \mathrm{w}& \mathrm{p}& \mathrm{q}& \mathrm{r}\end{array}\right]}^{\mathrm{T}}$$

(34)

To link these two vectors, the rotation matrix ${\mathrm{R}}_{\mathrm{xyz}}\left(\mathsf{\Phi },\mathsf{\theta },\mathsf{\Psi }\right)$ and a matrix of angular velocity transformations, ${\mathrm{T}}_{\mathrm{v}}\left(\mathsf{\Phi },\mathsf{\theta }\right)$ is used, derived from the inverse of the derivative of the Euler angle change rate. Thus in Equations (35)–(40) the dependencies between the vectors ${\mathrm{P}}_{\mathrm{p}}$ and ${\mathrm{P}}_{\mathrm{a}}$ are detailed.

$${\mathrm{v}}_{\mathrm{p}}= {\left[\begin{array}{ccc}\dot{\mathrm{x}}& \dot{\mathrm{y}}& \dot{\mathrm{z}}\end{array}\right]}^{\mathrm{T}}$$

(35)

$${\mathsf{\omega }}_{\mathrm{p}}= {\left[\begin{array}{ccc}\dot{\mathsf{\Phi }}& \dot{\mathsf{\theta }}& \dot{\mathsf{\Psi }}\end{array}\right]}^{\mathrm{T}}$$

(36)

$${\mathrm{v}}_{\mathrm{a}}= {\left[\begin{array}{ccc}\mathrm{u}& \mathrm{v}& \mathrm{w}\end{array}\right]}^{\mathrm{T}}$$

(37)

$${\mathsf{\omega }}_{\mathrm{a}}= {\left[\begin{array}{ccc}\mathrm{p}& \mathrm{q}& \mathrm{r}\end{array}\right]}^{\mathrm{T}}$$

(38)

$${\mathrm{v}}_{\mathrm{p}}={\mathrm{R}}_{\mathrm{xyz}}\left(\mathsf{\Phi },\mathsf{\theta },\mathsf{\Psi }\right)·{\mathrm{v}}_{\mathrm{a}}$$

(39)

$${\mathsf{\omega }}_{\mathrm{p}}={ \mathrm{T}}_{\mathrm{V}}\left(\mathsf{\Phi },\mathsf{\theta }\right)·{\mathsf{\omega }}_{\mathrm{a}}$$

(40)

The vectors ${\mathrm{v}}_{\mathrm{p}}$ and ${\mathsf{\omega }}_{\mathrm{p}}$ are the derivatives of the linear and angular positions of ${\mathrm{P}}_{\mathrm{p}}$, while ${\mathrm{v}}_{\mathrm{a}}$ and ${\mathsf{\omega }}_{\mathrm{a}}$ are the linear and angular velocities of the vector ${\mathrm{P}}_{\mathrm{a}}$. The matrix of angular velocity transformations ${\mathrm{T}}_{\mathrm{v}}\left(\mathsf{\Phi },\mathsf{\theta }\right)$ is constructed as described by Equation (41).

$${\mathrm{T}}_{\mathrm{v}}\left(\mathsf{\Phi },\mathsf{\theta }\right)= \left[\begin{array}{ccc}1& \mathrm{c}\left(\mathsf{\Phi }\right)\tan \left(\mathsf{\theta }\right)& \mathrm{c}\left(\mathsf{\Phi }\right)\tan \left(\mathsf{\theta }\right)\\ 0& \mathrm{c}\left(\mathsf{\Phi }\right)& -\mathrm{s}\left(\mathsf{\Phi }\right)\\ 0& \frac{\mathrm{s}\left(\mathsf{\Phi }\right)}{\mathrm{c}\left(\mathsf{\theta }\right)}& \frac{\mathrm{c}\left(\mathsf{\Phi }\right)}{\mathrm{c}\left(\mathsf{\theta }\right)}\end{array}\right]$$

(41)

Performing the multiplications leads to the kinematic model:

$$\{\begin{array}{l}\dot{\mathrm{x}}=\mathrm{uc}\left(\mathsf{\Psi }\right)\mathrm{c}\left(\mathsf{\theta }\right)-\mathrm{v}\left[\mathrm{c}\right(\mathsf{\Phi }\left)\mathrm{s}\right(\mathsf{\Psi })-\mathrm{c}(\mathsf{\Psi }\left)\mathrm{s}\right(\mathsf{\Phi }\left)\mathrm{s}\right(\mathsf{\theta }\left)\right]+\mathrm{w}\left[\mathrm{s}\right(\mathsf{\Phi }\left)\mathrm{s}\right(\mathsf{\Psi })+\mathrm{c}(\mathsf{\Phi }\left)\mathrm{s}\right(\mathsf{\Psi }\left)\mathrm{s}\right(\mathsf{\theta }\left)\right]\hfill \\ \dot{\mathrm{y}}=\mathrm{uc}\left(\mathsf{\theta }\right)\mathrm{c}\left(\mathsf{\Psi }\right)+\mathrm{v}\left[\mathrm{c}\right(\mathsf{\Phi }\left)\mathrm{c}\right(\mathsf{\Psi })+\mathrm{s}(\mathsf{\Phi }\left)\mathrm{s}\right(\mathsf{\theta }\left)\mathrm{s}\right(\mathsf{\Psi }\left)\right]-\mathrm{w}\left[\mathrm{c}\right(\mathsf{\Psi }\left)\mathrm{s}\right(\mathsf{\Phi })-\mathrm{c}(\mathsf{\Phi }\left)\mathrm{s}\right(\mathsf{\Psi }\left)\mathrm{s}\right(\mathsf{\theta }\left)\right]\hfill \\ \dot{\mathrm{z}}=-\mathrm{uc}\left(\mathsf{\theta }\right)+\mathrm{vc}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Phi }\right)+\mathrm{wc}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\theta }\right)\hfill \\ \dot{\mathsf{\Phi }}=\mathrm{p}+\mathrm{qs}\left(\mathsf{\Phi }\right)\mathrm{t}\left(\mathsf{\theta }\right)+\mathrm{rc}\left(\mathsf{\Phi }\right)\mathrm{t}\left(\mathsf{\theta }\right)\hfill \\ \dot{\mathsf{\theta }}=\mathrm{qc}\left(\mathsf{\Phi }\right)-\mathrm{rs}\left(\mathsf{\Phi }\right)\hfill \\ \dot{\mathsf{\Psi }}=\mathrm{q}\frac{\mathrm{s}\left(\mathsf{\Phi }\right)}{\mathrm{c}\left(\mathsf{\theta }\right)}+\mathrm{q}\frac{\mathrm{c}\left(\mathsf{\Phi }\right)}{\mathrm{c}\left(\mathsf{\theta }\right)}\hfill \end{array}$$

(42)

From Newton’s laws, the forces acting on the quadrotor can be determined. These will be denoted with vector ${\mathrm{F}}_{\mathrm{a}}$ and calculated as described in Equations (43) and (44).

$${\mathrm{F}}_{\mathrm{a}}={\mathrm{m}}_{\mathrm{q}}\left({\mathsf{\omega }}_{\mathrm{a}}\times {\mathrm{v}}_{\mathrm{a}}+{\dot{\mathrm{v}}}_{\mathrm{a}}\right)$$

(43)

$${\mathrm{F}}_{\mathrm{a}}={\left[\begin{array}{ccc}{\mathrm{f}}_{\mathrm{x}}& {\mathrm{f}}_{\mathrm{y}}& {\mathrm{f}}_{\mathrm{z}}\end{array}\right]}^{\mathrm{T}}$$

(44)

where m_q denotes the mass of the quadrotor, $“\times ”$ is the vector product of the linear and angular velocity relative to the quadrotor coordinate system, while ${\dot{\dot{\mathrm{v}}}}_{\mathrm{a}}$ is the linear acceleration.

Similar to the computation of the force, the angular velocity applied to the quadrotor will also be determined from Euler’s Equation. These velocities will be noted with ${\mathrm{M}}_{\mathrm{a}}$, and are strongly dependent on the inertia matrix I, as it is presented in Equations (45)–(47).

$${\mathrm{M}}_{\mathrm{a}}=\mathrm{I}·{\dot{\mathsf{\omega }}}_{\mathrm{a}}+{\mathsf{\omega }}_{\mathrm{a}}\times \left(\mathrm{I}·{\mathsf{\omega }}_{\mathrm{a}}\right)$$

(45)

$${\mathrm{M}}_{\mathrm{a}}={\left[\begin{array}{ccc}{\mathrm{m}}_{\mathrm{x}}& {\mathrm{m}}_{\mathrm{y}}& {\mathrm{m}}_{\mathrm{z}}\end{array}\right]}^{\mathrm{T}}$$

(46)

$$\mathrm{I}= \left[\begin{array}{ccc}{\mathrm{I}}_{\mathrm{x}}& 0& 0\\ 0& {\mathrm{I}}_{\mathrm{y}}& 0\\ 0& 0& {\mathrm{I}}_{\mathrm{z}}\end{array}\right]$$

(47)

Combining Equations (44) and (46), the dynamic model of the quadrotor relative to its own coordinate system can be expressed as:

$$\{\begin{array}{l}{\mathrm{f}}_{\mathrm{x}}=\mathrm{m}\left(\dot{\mathrm{u}}+\mathrm{qw}-\mathrm{rv}\right)\hfill \\ {\mathrm{f}}_{\mathrm{y}}=\mathrm{m}\left(\dot{\mathrm{v}}-\mathrm{pw}+\mathrm{ru}\right)\hfill \\ {\mathrm{f}}_{\mathrm{z}}=\mathrm{m}\left(\dot{\mathrm{w}}+\mathrm{pv}-\mathrm{qu}\right)\hfill \\ {\mathrm{m}}_{\mathrm{x}}={ \dot{\mathrm{p}}\mathrm{I}}_{\mathrm{x}}-{\mathrm{qrI}}_{\mathrm{y}}+{\mathrm{qrI}}_{\mathrm{z}}\hfill \\ {\mathrm{m}}_{\mathrm{y}}={\mathrm{prI}}_{\mathrm{x}}+{\dot{\mathrm{q}}\mathrm{I}}_{\mathrm{y}}-{\mathrm{prI}}_{\mathrm{z}}\hfill \\ {\mathrm{m}}_{\mathrm{z}}=-{\mathrm{pqI}}_{\mathrm{x}}+{\mathrm{pqI}}_{\mathrm{y}}+{\dot{\mathrm{r}}\mathrm{I}}_{\mathrm{z}}\hfill \end{array}$$

(48)

The forces and velocities described above can also be expressed by Equations (49) and (50).

$${\mathrm{F}}_{\mathrm{a}}={\mathrm{m}}_{\mathrm{q}}{\mathrm{gR}}_{\mathrm{xyz}}{\left(\mathsf{\Phi },\mathsf{\theta },\mathsf{\Psi }\right)}^{\mathrm{T}}·{\widehat{\mathrm{e}}}_{\mathrm{z}}-{\mathrm{f}}_{\mathrm{p}}·{\widehat{\mathrm{e}}}_3+{\mathrm{f}}_{\mathrm{v}}$$

(49)

$${\mathrm{M}}_{\mathrm{a}}={\mathsf{\tau }}_{\mathrm{a}}-{\mathrm{g}}_{\mathrm{a}}+{\mathsf{\tau }}_{\mathrm{v}}$$

(50)

In the above expression m_q means the total mass of the quadrotor, g is the gravitational acceleration, ${\widehat{\mathrm{e}}}_{\mathrm{z}}{ \mathrm{and} \widehat{\mathrm{e}}}_3$ are the unit vectors on the Z axis of the reference coordinate system, respectively of the coordinate system attached to the quadrotor. The element ${\mathrm{f}}_{\mathrm{p}}$ represents the total propulsion force developed by the engines, and ${\mathrm{f}}_{\mathrm{v}}={\left[\begin{array}{ccc}{\mathrm{f}}_{\mathrm{vx}}& {\mathrm{f}}_{\mathrm{vy}}& {\mathrm{f}}_{\mathrm{vz}}\end{array}\right]}^{\mathrm{T}}$ represents the disturbances or forces that are opposed to the rotation of each engine, caused by air currents. ${\mathsf{\tau }}_{\mathrm{a}}$ represents the angular velocity generated by the velocity differences of the four motors, while ${\mathsf{\tau }}_{\mathrm{v}}$ stands for the angular velocities produced by air currents on each motor, detailed in Equations (51) and (52). ${\mathrm{g}}_{\mathrm{a}}$ are the gyroscope moments caused by the combined velocities of the four motors. Given the fact that the inertia of the motors is negligible compared to the developed force, the gyroscopic moments may be neglected from Equation (50).

$${\mathsf{\tau }}_{\mathrm{a}}={\left[\begin{array}{ccc}{\mathsf{\tau }}_{\mathrm{x}}& {\mathsf{\tau }}_{\mathrm{y}}& {\mathsf{\tau }}_{\mathrm{z}}\end{array}\right]}^{\mathrm{T}}$$

(51)

$${\mathsf{\tau }}_{\mathrm{v}}={\left[\begin{array}{ccc}{\mathsf{\tau }}_{\mathrm{vx}}& {\mathsf{\tau }}_{\mathrm{vy}}& {\mathsf{\tau }}_{\mathrm{vz}}\end{array}\right]}^{\mathrm{T}}$$

(52)

Replacing these new Equations for forces and velocities, a new dynamic model is obtained:

$$\{\begin{array}{l}-{\mathrm{m}}_{\mathrm{q}}\mathrm{g}\mathrm{s}\left(\mathsf{\theta }\right)+{\mathrm{f}}_{\mathrm{vx}}={\mathrm{m}}_{\mathrm{q}}\left(\dot{\mathrm{u}}+\mathrm{qw}-\mathrm{rv}\right)\hfill \\ {\mathrm{m}}_{\mathrm{q}}\mathrm{g}\mathrm{c}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Phi }\right)+{\mathrm{f}}_{\mathrm{vy}}={\mathrm{m}}_{\mathrm{q}}\left(\dot{\mathrm{v}}-\mathrm{pw}+\mathrm{ru}\right)\hfill \\ {\mathrm{m}}_{\mathrm{q}}\mathrm{g}\mathrm{c}\left(\mathsf{\theta }\right)\mathrm{c}\left(\mathsf{\Phi }\right)+{\mathrm{f}}_{\mathrm{vz}}-{\mathrm{f}}_{\mathrm{p}}={\mathrm{m}}_{\mathrm{q}}\left(\dot{\mathrm{w}}+\mathrm{pv}-\mathrm{qu}\right)\hfill \\ {\mathsf{\tau }}_{\mathrm{x}}+{\mathsf{\tau }}_{\mathrm{vx}}={\dot{\mathrm{p}}\mathrm{I}}_{\mathrm{x}}-{\mathrm{qrI}}_{\mathrm{y}}+{\mathrm{qrI}}_{\mathrm{z}}\hfill \\ {\mathsf{\tau }}_{\mathrm{y}}+{\mathsf{\tau }}_{\mathrm{vy}}={\mathrm{prI}}_{\mathrm{x}}+{\dot{\mathrm{q}}\mathrm{I}}_{\mathrm{y}}-{\mathrm{prI}}_{\mathrm{z}}\hfill \\ {\mathsf{\tau }}_{\mathrm{z}}+{\mathsf{\tau }}_{\mathrm{vz}}=-{\mathrm{pqI}}_{\mathrm{x}}+{\mathrm{pqI}}_{\mathrm{y}}+{\dot{\mathrm{r}}\mathrm{I}}_{\mathrm{z}}\hfill \end{array}$$

(53)

In order to control the quadrotor, the dependence between the propulsion force ${\mathrm{f}}_{\mathrm{p}}$, velocity ${\mathsf{\tau }}_{\mathrm{a}},$and the motor’s angular velocities ${\mathsf{\Omega }}_{\mathrm{a}}=\left[\begin{array}{cccc}{\mathsf{\Omega }}_1& {\mathsf{\Omega }}_2& {\mathsf{\Omega }}_3& {\mathsf{\Omega }}_4\end{array}\right]$ needs to be introduced in the model, using Equation (54).

$$\{\begin{array}{l}{\mathrm{f}}_{\mathrm{p}}=\mathrm{b}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right) \hfill \\ {\mathsf{\tau }}_{\mathrm{x}}=\mathrm{b}·\mathrm{l}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_4^2\right)\hfill \\ {\mathsf{\tau }}_{\mathrm{y}}=\mathrm{b}·\mathrm{l}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_4^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2\right)\hfill \\ {\mathsf{\tau }}_{\mathrm{z}}=\mathrm{d}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_4^2\right)\hfill \end{array}$$

(54)

where b is a propulsion coefficient and d is the aerodynamic resistance coefficient. The term l represents the distance from the center of gravity of the quadrotor to the center of rotation of the engine. This term is equal for all four arms of the quadrotor. In addition, replacing the terms obtained from Equation (54) in (53) leads to a new expression of the dynamic model, given by:

$$\{\begin{array}{l}-{\mathrm{m}}_{\mathrm{q}}\mathrm{g} \mathrm{s}\left(\mathsf{\theta }\right)+{\mathrm{f}}_{\mathrm{vx}}={\mathrm{m}}_{\mathrm{q}}\left(\dot{\mathrm{u}}+\mathrm{qw}-\mathrm{rv}\right)\hfill \\ {\mathrm{m}}_{\mathrm{q}}\mathrm{g} \mathrm{c}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Phi }\right)+{\mathrm{f}}_{\mathrm{vy}}={\mathrm{m}}_{\mathrm{q}}\left(\dot{\mathrm{v}}-\mathrm{pw}+\mathrm{ru}\right)\hfill \\ {\mathrm{m}}_{\mathrm{q}}\mathrm{g}\mathrm{c}\left(\mathsf{\theta }\right)\mathrm{c}\left(\mathsf{\Phi }\right)+{\mathrm{f}}_{\mathrm{vz}}-\mathrm{b}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)={\mathrm{m}}_{\mathrm{q}}\left(\dot{\mathrm{w}}+\mathrm{pv}-\mathrm{qu}\right)\hfill \\ \mathrm{b} · \mathrm{l}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_4^2\right)+{\mathsf{\tau }}_{\mathrm{vx}}={\dot{\mathrm{p}}\mathrm{I}}_{\mathrm{x}}-{\mathrm{qrI}}_{\mathrm{y}}+{\mathrm{qrI}}_{\mathrm{z}}\hfill \\ \mathrm{b} · \mathrm{l}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_4^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_2^3\right)+{\mathsf{\tau }}_{\mathrm{vy}}={\mathrm{prI}}_{\mathrm{x}}+{\dot{\mathrm{q}}\mathrm{I}}_{\mathrm{y}}-{\mathrm{prI}}_{\mathrm{z}}\hfill \\ \mathrm{d}\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_4^2\right)+{\mathsf{\tau }}_{\mathrm{vz}}=-{\mathrm{pqI}}_{\mathrm{x}}+{\mathrm{pqI}}_{\mathrm{y}}+{\dot{\mathrm{r}}\mathrm{I}}_{\mathrm{z}}\hfill \end{array}$$

(55)

This model will be used as predictor in the control structure.

2.3. Quadrotor State Space Model Used for Controller Design

The next step consists in the model design in a state space form, in order to easily apply the controller design methods. Therefore, the state vector $\mathbb{X}$, the input vector $\mathrm{u}$, and the output vector $\mathrm{y}$ will be chosen as it is presented below:

$$\mathbb{X}={\left[\begin{array}{cccccccccccc}\mathsf{\Phi }& \mathsf{\theta }& \mathsf{\Psi }& \mathrm{p}& \mathrm{q}& \mathrm{r}& \mathrm{u}& \mathrm{v}& \mathrm{w}& \mathrm{x}& \mathrm{y}& \mathrm{z}\end{array}\right]}^{\mathrm{T}}$$

(56)

$$\mathrm{u}={\left[\begin{array}{cccc}{\mathrm{f}}_{\mathrm{p}}& {\mathsf{\tau }}_{\mathrm{x}}& {\mathsf{\tau }}_{\mathrm{y}}& {\mathsf{\tau }}_{\mathrm{z}}\end{array}\right]}^{\mathrm{T}}$$

(57)

$$\mathrm{y}={\left[\begin{array}{cccc}\mathsf{\Phi }& \mathsf{\theta }& \mathsf{\Psi }& {\mathrm{f}}_{\mathrm{p}}\end{array}\right]}^{\mathrm{T}}$$

(58)

Using this state vector $\mathbb{X}$ and Equations (42) and (55), one can determine the derivative of this state vector, $\dot{\mathbb{X}}$:

$$\{\begin{array}{l}\dot{\mathsf{\Phi }}=\mathrm{p}+\mathrm{qs}\left(\mathsf{\Phi }\right)\mathrm{t}\left(\mathsf{\theta }\right)+\mathrm{rc}\left(\mathsf{\Phi }\right)\mathrm{t}\left(\mathsf{\theta }\right)\hfill \\ \dot{\mathsf{\theta }}=\mathrm{qc}\left(\mathsf{\Phi }\right)-\mathrm{rs}\left(\mathsf{\Phi }\right)\hfill \\ \dot{\mathsf{\Psi }}=\mathrm{q}\frac{\mathrm{s}\left(\mathsf{\Phi }\right)}{\mathrm{c}\left(\mathsf{\theta }\right)}+\mathrm{r}\frac{\mathrm{c}\left(\mathsf{\Phi }\right)}{\mathrm{c}\left(\mathsf{\theta }\right)}\hfill \\ \dot{\mathrm{p}}=\frac{{\mathrm{I}}_{\mathrm{y}}-{\mathrm{I}}_{\mathrm{z}}}{{\mathrm{I}}_{\mathrm{x}}}\mathrm{qr}+\frac{{\mathsf{\tau }}_{\mathrm{x}}+{\mathsf{\tau }}_{\mathrm{vx}}}{{\mathrm{I}}_{\mathrm{x}}}\hfill \\ \dot{\mathrm{q}}=\frac{{\mathrm{I}}_{\mathrm{z}}-{\mathrm{I}}_{\mathrm{x}}}{{\mathrm{I}}_{\mathrm{y}}}\mathrm{pr}+\frac{{\mathsf{\tau }}_{\mathrm{y}}+{\mathsf{\tau }}_{\mathrm{vy}}}{{\mathrm{I}}_{\mathrm{y}}}\hfill \\ \dot{\mathrm{r}}=\frac{{\mathrm{I}}_{\mathrm{x}}-{\mathrm{I}}_{\mathrm{y}}}{{\mathrm{I}}_{\mathrm{z}}}\mathrm{pq}+\frac{{\mathsf{\tau }}_{\mathrm{z}}+{\mathsf{\tau }}_{\mathrm{vz}}}{{\mathrm{I}}_{\mathrm{z}}}\hfill \\ \dot{\mathrm{u}}=\mathrm{rv}-\mathrm{qw}-\mathrm{gs}\left(\mathsf{\theta }\right)+\frac{{\mathrm{f}}_{\mathrm{vx}}}{\mathrm{m}}\hfill \\ \dot{\mathrm{v}}=\mathrm{pw}-\mathrm{ru}+\mathrm{gs}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\theta }\right)+\frac{{\mathrm{f}}_{\mathrm{vy}}}{\mathrm{m}}\hfill \\ \dot{\mathrm{w}}=\mathrm{qu}-\mathrm{pv}+\mathrm{gc}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\theta }\right)+\frac{{\mathrm{f}}_{\mathrm{vz}}-{\mathrm{f}}_{\mathrm{p}}}{\mathrm{m}}\hfill \\ \dot{\mathrm{x}}=\mathrm{uc}\left(\mathsf{\Psi }\right)\mathrm{c}\left(\mathsf{\theta }\right)-\mathrm{v}\left[\mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\Psi }\right)-\mathrm{c}\left(\mathsf{\Psi }\right)\mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\right]+\mathrm{w}\left[\mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\Psi }\right)+\mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\Psi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\right]\hfill \\ \dot{\mathrm{y}}=\mathrm{uc}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Psi }\right)+\mathrm{v}\left[\mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\Psi }\right)+\mathrm{s}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Psi }\right)\right]-\mathrm{w}\left[\mathrm{c}\left(\mathsf{\Psi }\right)\mathrm{s}\left(\mathsf{\Phi }\right)-\mathrm{c}\left(\mathsf{\Phi }\right)\mathrm{s}\left(\mathsf{\Psi }\right)\mathrm{s}\left(\mathsf{\theta }\right)\right]\hfill \\ \dot{\mathrm{z}}=-\mathrm{us}\left(\mathsf{\theta }\right)+\mathrm{vc}\left(\mathsf{\theta }\right)\mathrm{s}\left(\mathsf{\Phi }\right)+\mathrm{wc}\left(\mathsf{\Phi }\right)\mathrm{c}\left(\mathsf{\theta }\right)\hfill \end{array}$$

(59)

As can be seen from Equation (59), the system is strongly nonlinear, presenting major problems in the design of a control system based on models. In order to be linearized, a Jacobian matrix is used, at certain chosen equilibrium points. Given that it is desired that in the absence of a command the system be maintained at a fixed point at a predetermined altitude, the equilibrium points are chosen as described below.

$${\mathbb{X}}_{\mathrm{e}}={\left[\begin{array}{cccccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0& {\mathrm{x}}_{\mathrm{e}}& {\mathrm{y}}_{\mathrm{e}}& {\mathrm{z}}_{\mathrm{e}}\end{array}\right]}^{\mathrm{T}}$$

(60)

$${\mathrm{u}}_{\mathrm{e}}={\left[\begin{array}{cccc}{\mathrm{m}}_{\mathrm{q}}·\mathrm{g}& 0& 0& 0\end{array}\right]}^{\mathrm{T}}$$

(61)

where $\mathrm{g}=9.8 \raisebox{1ex}{$\mathrm{m}$}\!\left/ \!\raisebox{-1ex}{${\mathrm{s}}^2$}\right.$ is the gravitational acceleration and m_q is the total mass of the quadrotor.

Also, since the trigonometric dependencies between the system states do not disappear even after the linearization by the Jacobian method, a preliminary simplification is made. Thus, in order to eliminate the trigonometric functions from the system model, all the values of the sine functions are approximated with their argument, respectively the cosine functions with 1. The approximate model, resulting from the simplification, has the form as described in (62).

$$\{\begin{array}{l}\dot{\mathsf{\Phi }}=\mathrm{p}+\mathrm{q}\mathsf{\Phi }\mathsf{\theta }+\mathrm{r}\mathsf{\theta }\hfill \\ \dot{\mathsf{\theta }}=\mathrm{q}-\mathrm{r}\mathsf{\Phi }\hfill \\ \dot{\mathsf{\Psi }}=\mathrm{q}\mathsf{\Phi }+\mathrm{r}\hfill \\ \dot{\mathrm{p}}=\frac{{\mathrm{I}}_{\mathrm{y}}-{\mathrm{I}}_{\mathrm{z}}}{{\mathrm{I}}_{\mathrm{x}}}\mathrm{qr}+\frac{{\mathsf{\tau }}_{\mathrm{x}}+{\mathsf{\tau }}_{\mathrm{vx}}}{{\mathrm{I}}_{\mathrm{x}}}\hfill \\ \dot{\mathrm{q}}=\frac{{\mathrm{I}}_{\mathrm{z}}-{\mathrm{I}}_{\mathrm{x}}}{{\mathrm{I}}_{\mathrm{y}}}\mathrm{pr}+\frac{{\mathsf{\tau }}_{\mathrm{y}}+{\mathsf{\tau }}_{\mathrm{vy}}}{{\mathrm{I}}_{\mathrm{y}}}\hfill \\ \dot{\mathrm{r}}=\frac{{\mathrm{I}}_{\mathrm{x}}-{\mathrm{I}}_{\mathrm{y}}}{{\mathrm{I}}_{\mathrm{z}}}\mathrm{pq}+\frac{{\mathsf{\tau }}_{\mathrm{z}}+{\mathsf{\tau }}_{\mathrm{vz}}}{{\mathrm{I}}_{\mathrm{z}}}\hfill \\ \dot{\mathrm{u}}=\mathrm{rv}-\mathrm{qw}-\mathrm{g}\mathsf{\theta }+\frac{{\mathrm{f}}_{\mathrm{vx}}}{{\mathrm{m}}_{\mathrm{q}}}\hfill \\ \dot{\mathrm{v}}=\mathrm{pw}-\mathrm{ru}+\mathrm{g}\mathsf{\Phi }+\frac{{\mathrm{f}}_{\mathrm{vy}}}{{\mathrm{m}}_{\mathrm{q}}}\hfill \\ \dot{\mathrm{w}}=\mathrm{qu}-\mathrm{pv}+\mathrm{g}+\frac{{\mathrm{f}}_{\mathrm{vz}}-{\mathrm{f}}_{\mathrm{p}}}{{\mathrm{m}}_{\mathrm{q}}}\hfill \\ \dot{\mathrm{x}}=\mathrm{u}-\mathrm{v}\left(\mathsf{\Psi }-\mathsf{\Phi }\mathsf{\theta }\right)+\mathrm{w}\left(\mathsf{\Phi }\mathsf{\Psi }+\mathsf{\theta }\right)\hfill \\ \dot{\mathrm{y}}=\mathrm{u}\mathsf{\Psi }+\mathrm{v}\left(1+\mathsf{\Phi }\mathsf{\theta }\mathsf{\Psi }\right)-\mathrm{w}\left(\mathsf{\Phi }-\mathsf{\Psi }\mathsf{\theta }\right)\hfill \\ \dot{\mathrm{z}}=-\mathrm{u}\mathsf{\theta }+\mathrm{v}\mathsf{\Phi }+\mathrm{w}\hfill \end{array}$$

(62)

In the state space form, the system is $\dot{\mathbb{X}}=\mathrm{h}\left(\mathbb{X}, \mathbf{u}\right).$ Applying the linearization by the Jacobian matrix method and using the equilibrium points expressed in (60) and (61), the linearized state space system became:

$$\{\begin{array}{l}\dot{\mathbb{X}}={\mathrm{A}}_{\mathrm{e}}·\mathbb{X}+{\mathrm{B}}_{\mathrm{e}}·\mathrm{u}\hfill \\ \mathrm{y}=\mathrm{C}·\mathbb{X}\hfill \end{array}$$

(63)

with

$${\mathrm{A}}_{\mathrm{e}}=\frac{\partial \mathrm{h}\left(\mathbb{X}, \mathrm{u}\right)}{\partial \mathbb{X}}|\begin{array}{c}\mathbb{X}={\mathbb{X}}_{\mathrm{e}}\\ \mathrm{u}={\mathrm{u}}_{\mathrm{e}}\end{array}=\left[\begin{array}{cccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& -\mathrm{g}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ \mathrm{g}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\end{array}\right]$$

(64)

$${\mathrm{B}}_{\mathrm{e}}=\frac{\partial \mathrm{h}\left(\mathbb{X}, \mathrm{u}\right)}{\partial \mathrm{u}}|\begin{array}{c}\mathbb{X}={\mathbb{X}}_{\mathrm{e}}\\ \mathrm{u}={\mathrm{u}}_{\mathrm{e}}\end{array}=\left[\begin{array}{cccc}0& 0& 0& 1\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& \frac{1}{{\mathrm{I}}_{\mathrm{x}}}& 0& 0\\ 0& 0& \frac{1}{{\mathrm{I}}_{\mathrm{y}}}& 0\\ 0& 0& 0& \frac{1}{{\mathrm{I}}_{\mathrm{z}}}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ \frac{1}{{\mathrm{m}}_{\mathrm{q}}}& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$$

(65)

$$\mathrm{C}= \left[\begin{array}{cccccccccccc}0& 0& 0& 0& 0& 0& 0& \mathrm{g}& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& -\mathrm{g}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& -1/{\mathrm{m}}_{\mathrm{q}}& 0& 0& 0\end{array}\right]$$

(66)

The same model can be described as a system of Equations as indicated in (67).

$$\{\begin{array}{l}\dot{\mathsf{\Phi }}=\mathrm{p}\hfill \\ \dot{\mathsf{\theta }}=\mathrm{q}\hfill \\ \dot{\mathsf{\Psi }}=\mathrm{r}\hfill \\ \dot{\mathrm{p}}=\frac{{\mathsf{\tau }}_{\mathrm{x}}+{\mathsf{\tau }}_{\mathrm{vx}}}{{\mathrm{I}}_{\mathrm{x}}}\hfill \\ \dot{\mathrm{q}}=\frac{{\mathsf{\tau }}_{\mathrm{y}}+{\mathsf{\tau }}_{\mathrm{vy}}}{{\mathrm{I}}_{\mathrm{y}}}\hfill \\ \dot{\mathrm{r}}=\frac{{\mathsf{\tau }}_{\mathrm{z}}+{\mathsf{\tau }}_{\mathrm{vz}}}{{\mathrm{I}}_{\mathrm{z}}} \hfill \\ \dot{\mathrm{u}}=-\mathrm{g}\mathsf{\theta }+\frac{{\mathrm{f}}_{\mathrm{vx}}}{{\mathrm{m}}_{\mathrm{q}}}\hfill \\ \dot{\mathrm{v}}=\mathrm{g}\mathsf{\Phi }+\frac{{\mathrm{f}}_{\mathrm{vy}}}{{\mathrm{m}}_{\mathrm{q}}}\hfill \\ \dot{\mathrm{w}}=\frac{{\mathrm{f}}_{\mathrm{vz}}-{\mathrm{f}}_{\mathrm{p}}}{{\mathrm{m}}_{\mathrm{q}}}\hfill \\ \dot{\mathrm{x}}=\mathrm{u}\hfill \\ \dot{\mathrm{y}}=\mathrm{v}\hfill \\ \dot{\mathrm{z}}=\mathrm{w}\hfill \end{array}$$

(67)

2.4. Controller Design

The controller design method uses the linear model of the system (63). Considering that references for the orientation of the quadrotor and for the altitude of flight will be transmitted from the remote controller, and the system inputs depend on the angular velocities of the four engines, a number of four controllers will be implemented for each direction of movement. Each controller can be designed with any tuning algorithm, ensuring the cancellation of the steady state error and a short settling time. An interesting choice is presented for example in [44]. If advanced controller tuning methods are used, the performances could be increased. The idea of the present work is to implement a very low-cost quadrotor, with the simplest control algorithm, but with results comparable with advanced control methods. With this regard a simple PID controller is designed for each rotor, using the classical root locus method [45]. For this method, given the characteristic polynomial of the closed-loop system, the parameters of controller are chosen depending on the location of the poles of the system. Overshoot, settling time and steady state error cancellation are imposed for each controller. Figure 1 illustrates the block diagram of the control strategy chosen for this quadrotor, with the PID blocks detailed in Figure 2. The proposed feedback control requires feedback signals and disturbance identification. To obtain these signals, a sensor with 9 degrees of freedom, consisting of an accelerometer, a gyroscope, and a magnetometer is used. Signals from this sensor must be processed because they suffer from noise disturbance and other drawbacks. For example the gyroscope has a flowing bias. This inconvenient is mitigated by the estimator. Both data filtering and estimating the orientation of the aerial vehicle are realized with the quaternion representation of the estimator (27,28). The nonlinear dynamic model (55) is used as predictor in the control structure.

In Figure 2 the signals are denoted as follows: r(k) is the reference signal at current iteration k; c(k) represents the control signal at this current iteration k; y(k) is the output of the system, measured by sensors at iteration k; e(k) is the error signal at iteration k; K_p, K_i, and K_d represent the proportionality, integration, and derivation constant, respectively. Regarding the angular velocities of the motors, it is necessary to ensure that they behave according to the control signals received from the angular position and altitude controllers. In view of the microcontroller’s processing capacity and the relatively large dimensions of the program used to obtain the inclination angles and the control law previously determined, four electronic speed control modules (ESCs) will be used to control the angular velocities of the motors.

The verification of the designed controllers was first performed through a numerical simulation. The non-linear model from Equation (55) was used to carry out all simulations. Several simulation scenarios were adjusted in order to set the simulation closer to reality. Furthermore, some restrictions related to the actuators were applied based on real data measurements. The delay of the actuators was implemented because of the use of the electronic speed controller (ESC). Moreover, sensor noise was implemented to the measured feedback signals. The evaluation of the designed controller was done both in disturbance free, constant disturbance, and real disturbance conditions. In each case the quadrotor has to follow the same trajectory, including takes off, flying from point A to B, and rotation around the Z axis. As quality indicators chosen to discuss the efficiency of the proposed algorithm are the steady state position error, overshoot, and settling time. In all cases the proposed simple control structure exhibit very similar behavior to advanced, expensive solutions.

3. Results

Figure 3 presents the resulted low-cost quadrotor UAV. It has four motors controlled by electronic speed controllers rotating as described in Figure 4. Each motor is mounted on a plastic arm, which in turn is attached to the carbon fiber central structure. All pieces were chosen so that the assembly has the lowest weight and, at the same time, to maintain the condition of the center of gravity described in the previous section.

In each step of the design, the aim was to understand the functionality of each component of the system and to describe the relationships between them using block diagrams. In this regard Figure 5 and Figure 6 detail the block diagrams of each subsystem, highlighting the type of data provided by/for each element.

In accordance with these block schemes and the dimensions imposed by the mechanical elements, a series of electrical components were chosen. These have been selected so that they can achieve the specifications of the desired control, allow flexibility in resolving errors and have low cost. Also, from the point of view of the processing capacity and the number of input and output signals, respectively, a microcontroller was chosen that satisfies these conditions.

The wiring diagram and the implemented UAV system are presented in Figure 7. The corresponding remote controller schemes are in Figure 8, where (a) represents the wiring diagram designed in Eagle, while (b) is the implemented circuit.

Measurements were realized without using the developed estimator. Figure 9 presents the raw results of the gyroscope, accelerometer and magnetometer for a linear movement. It can be concluded that in the case of noisy signals, such an approach is not usable in a feedback control structure. It is obvious the necessity of the estimator.

The quaternion-based estimation algorithm described in the previous section was implemented on the microcontroller. In order to test the obtained system, a reference sequence of the form: 0, maximum value to the right, maximum value to the left was applied. The obtained results are plotted in Figure 10.

The designed nonlinear model was tested, obtaining the results from Figure 11.

The designed control algorithms were also implemented on the microcontroller. The obtained results in the worst-case scenario, windy conditions, are plotted on Figure 12, presenting the response of the closed loop system to a step reference on each of the four directions of movement.

The performance was measured for different operation scenarios, including different step inputs on each axis, wind-free and windy conditions. The results for one of the “classical” scenarios—16° step input for angular position on X and Y axis and 45° on Z axis, 5 m altitude, with relatively high wind speed—are presented in

Table 1. The obtained performance measures.

Movement	Overshoot [%]	Settling Time [sec]	Steady State Position Error
Front-back	13.75	0.65	0
Left-right	12.5	0.51	0
Rotation around the central axis	0	1.2	0
Up-down	2	0.8	0

, highlighting good performances. All these results are comparable with the results of advanced control algorithms in [25,26,27,28,29,30,31,32,33,34,35,36], without needing expensive hardware equipment. In [26], where the studied quadrotor is similar with our prototype, a LQR controller is used for altitude and a PD controller for position, resulting in a settling time for a step input between 2 and 3.7 s and overshoot 13–20%. Using a backstepping controller combined with the PD, the settling time varies between 2.10 and 3.70 s and the overshoot between 12 and 14%. The LQR controller used both for altitude and position, the settling time are 2.7–3.35 s, while the overshoot is 19–25%. The combination of the backstepping controller with LQR leads to values varying between 2.7–3.3 s, overshoot 15–25%. In our experiments the overshot does not exceed 13.75% neither in worst case and the largest settling time is 1.2 s. The model identification adaptive control (MIAC) used in [28] leads to settling time between 0.8 and 1.4 s, while with the Model Reference Adaptive Control (MRAC) from the same study, the achieved settling time is of 0.75–2 s, very close to our values. The main advantages of these two (MIAC and MRAC) controllers are the overshoot cancellation, but the cost is the control effort. Analyzing the active disturbance rejection controller designed in [11], the presented settling times are 0.85–1.5 s for a 20° step input, overshoot is 7–25%, comparable with our results. The advantage of the high-order sliding mode-based fixed-time active disturbance rejection control from [11] is that it tracks the unknown disturbances in about 3 s.

Comparing our results with the results of the bioinspired controller from [10], the present results are still competitive. Moreover, imposing different settling time and overshoot in the design stage, it is possible to set a different transient response. Reducing the overshot will increase the settling time and vice versa. Obviously, designing an advanced controller could increase the performances, but the idea of present work was to analyze the most simple algorithm, a PID controller.

4. Conclusions

The present research is focused on a low-cost, but performing UAV system design. Taken as a whole, such a flight system presents great difficulties in obtaining positioning data, in particular due to their complex determination or estimation algorithms. In addition to the high complexity of the estimation algorithms, the problem of measurement errors and the resolution of the sensors must often be taken into consideration, so that the control structures are provided with the most accurate data. The offered solution is the quaternion-based estimation. In addition, the tuning of the proportional, integrative, and derivative terms of the control laws is another major problem of the UAV system. Also, the nonlinearities present in such a system introduce challenging problems.

The prototype described in the previous sections offers solution for all these problems.

As future works a global positioning system (GPS) would be added to the equipment model in order to acquire more functionalities.