Unmanned Aerial Vehicles

Abstract

The multi-DOF dynamic model of unmanned aerial vehicles (UAVs) is a highly nonlinear one and its control can be performed again with (i) global linearization control methods, (ii) local linearization control methods and (iii) Lyapunov analysis-based methods. In approach (i) the dynamic model of the UAV is transformed into an equivalent linear description through the application of a change of variables (diffeomorphisms). In (ii) the nonlinear model of the UAV is decomposed into local linear models for which linear feedback controllers are designed and next the aim is to select the feedback control gains so as to assure the global asymptotic stability of the control loop. In (iii) the objective is to define an energy function for the UAV (Lyapunov function) and to demonstrate that through suitable selection of the feedback control the first derivative of the energy function is always negative and thus the global stability of the control loop is assured. The latter approach is particularly suitable for model-free control of UAVs and takes the form of adaptive control methods. This chapter analyzes the aforementioned control approaches for UAVs and proves global asymptotic stability for all considered control approaches (i) to (iii). The robustness of the aforementioned control methods to model uncertainty and external perturbations is confirmed. Besides elaborated nonlinear filtering approaches are developed that allow for accurate estimation of the state vector of the UAVs through the processing of measurements coming from a limited number of sensors. In particular this chapter treats the following topics: (a) Control of UAVs based on global linearization methods, (b) Control of UAVs based on approximate linearization methods.

9.1 Chapter Overview

The present chapter treats the following topics: (a) Control of UAVs based on global linearization methods, (b) Control of UAVs based on approximate linearization methods.

With reference to (a) the chapter uses a differential flatness theory-based implementation of the Kalman Filter (known as Derivative-free nonlinear Kalman Filter) for developing a robust controller which can be applied to quadropters. The control problem for quadropters is non-trivial and becomes further complicated if this robot is subject to model uncertainties and external disturbances. Using differential flatness theory it is shown that the model of a quadropter can be transformed into linear canonical form. For the linearized equivalent of the quadropter it is shown that a state feedback controller can be designed. Since certain elements of the state vector of the linearized system cannot be measured directly, it is proposed to estimate them with the use of the previously analyzed Derivative-free nonlinear Kalman Filter. Moreover, by redesigning the Kalman Filter as a disturbance observer, it is is shown that one can estimate simultaneously external disturbances terms that affect the quadropter or disturbance terms which are associated with parametric uncertainty.

With reference to (b) the chapter applies nonlinear H-infinity (optimal) control the dynamic model of 6-DOF UAVs. First, the dynamic model of the UAV undergoes approximate linearization, through Taylor series expansion, round local operating points which are defined at each time instant by the present value of the system’s state vector and the last value of the control input that was exerted on it. The linearization procedure requires the computation of Jacobian matrices at the aforementioned operating points. Next, for the linearized equivalent model of the UAV, an H-infinity feedback control loop is designed. The computation of the optimal control input requires the solution of an algebraic Riccati equation at each iteration of the control algorithm. The known robustness properties of H-infinity control enable compensation of model uncertainty and rejection of the perturbation terms that affect the UAV. The stability of the control loop is proven through Lyapunov analysis.

9.2 Control of UAVs Based on Global Linearization

9.2.1 Outline

Quadrotors are four-rotor helicopters characterized by a nonlinear 6-DOF unstable dynamical model. To achieve autonomous navigation of the quadrotors it is necessary to design efficient control algorithms that will exhibit robustness to parametric uncertainties and to external disturbances. One can cite several results on quadrotors’ control. An approach for quadrotors’ control that is based on the transformation of their dynamical model in the linear canonical form and which is consequently directly associated with differential flatness theory has been given in [576]. Moreover, in [6] a flatness-based control approach is applied to quadrotors’ motion control. A predictive controller complemented by an $H_{\infty }$ term for additional robustness has been analyzed and tested in the quadrotor’s flight control problem in [401, 403]. In [55] motion control of the quadrotor was implemented using controllers of the LQR-type and of the PID-type, while Kalman Filtering has been used to provide position estimates out of a visual measurements system. In [77] two control strategies are employed as baseline controllers for the quadrotor’s model: a LQR controller which is based on a linearized model of the quadrotor and a Sliding Mode Controller which is based on a nonlinear model of the quadrotor. Moreover, differential flatness theory has been used for trajectory planning. In [261] and in [135] adaptive control schemes have been proposed for the quadrotor’s model. The stability of the control loop is confirmed through the Lyapunov approach. In [48] quadrotor’s control with the use of a sliding-mode controller and a sliding-mode disturbance observer has been proposed.

In this section a new control method is developed first for quadrotors after applying global linearization of the UAV’s dynamic model. The method comprises differential flatness theory together with the use of a disturbance observer. This state and perturbations observer is also in accordance to differential flatness theory and is the so-called Derivative-free nonlinear Kalman Filter. The differential flatness theory-based design of the controller uses a change of coordinates (diffeomorphism) that transforms the state-space equation of the quadrotor’s model into the linear canonical (Brunovsky) form [57, 145, 322, 450, 476, 572]. For the linearized equivalent of the quadrotor it is easier to design a state feedback controller using techniques for linear feedback controllers’ synthesis. To provide the quadrotor’s control loop with additional robustness a disturbance observer is used. The disturbance observer makes use of the standard Kalman Filter recursion on the linearized model of the quadrotor. It is capable of estimating simultaneously the quadrotor’s linear and rotational velocities, as well as the vector of disturbances that affect the quadrotor’s model without the need to compute Jacobian matrices. The accurate estimation of the disturbance inputs enables to introduce an additional control term that compensates for the disturbances’ effects. The accurate tracking of reference trajectories that is achieved by the quadrotor despite the existence of external disturbances is shown in simulation experiments.

As already analyzed, differential flatness theory has specific advantages when used in nonlinear control systems [57, 145, 322, 457, 476, 572]. Through an exact linearization of the system’s state-space description, one can avoid the use of linear models with local validity in the controller’s design. The controller performs efficiently despite the change of operating points. After the design of such a state feedback controller, one can consider the inclusion in the control loop of supplementary control terms that provide additional robustness. As mentioned above, it is also possible to use a disturbance estimator-based auxiliary control input for compensating for the effects of disturbances in the feedback control loop. Moreover, the use of differential flatness theory in the design of state estimators and filters has also several strong points. One can perform estimation of the complete state vector of the system without the need to compute partial derivatives and Jacobian matrices. Moreover, by avoiding numerical errors which are due to approximate linearization of the system’s dynamic model linear estimation algorithms can be implemented. In the case of Kalman Filter this means that one can perform state estimation with the use of the standard Kalman Filter recursion, thus preserving the method’s optimality features and providing state estimates of improved precision (e.g. comparing to Extended Kalman Filtering).

9.2.2 Kinematic Model of the Quadropter

9.2.2.1 State-Space Model of the UAV

Two reference frames are defined [401, 403]. The first one $B=[B_1,B_2,B_3]$ is attached to the quadropter’s body, whereas the second $E=[E_x,E_y, E_z]$ is considered to be an inertial coordinates system. As shown in Fig. 9.1, the Euler angles defining rotation round the axes of the body-fixed frame $B_1$, $B_2$ and $B_3$ are defined as $\theta$, $\phi$ and $\psi$, respectively. The two reference frames are connected to each other through a rotation matrix

$$\begin{aligned} \begin{array}{ccc} R=\begin{pmatrix} C{\psi }C{\theta } &{} C{\psi }S{\theta }S{\phi }-S{\psi }C{\phi } &{} C{\psi }S{\theta }C{\phi }+S{\psi }S{\phi } \\ S{\psi }C{\theta } &{} S{\psi }S{\theta }S{\phi }+C{\psi }C{\phi } &{} S{\psi }S{\theta }C{\phi }-C{\psi }S{\phi } \\ -S{\theta } &{} C{\theta }S{\phi } &{} C{\theta }C{\phi } \end{pmatrix} \end{array} \end{aligned}$$

(9.1)

where $C=cos(\cdot )$ and $S=sin(\cdot )$.

Fig. 9.1

Reference axes for the quadropter

The connection between velocities in the two reference frames is as follows:

$$\begin{aligned} \begin{array}{c} V_E=R{\cdot }{V_B} \end{array} \end{aligned}$$

(9.2)

where $V_E=[u_E,v_E, w_E]$ and $V_B=[u_B,v_B, w_B]$ are the linear velocity vectors expressed in the two reference frames. About the angular velocities in the two reference frames the following relation holds

$$\begin{aligned} \begin{array}{c} \dot{\eta }={W^{-1}}\omega \end{array} \end{aligned}$$

(9.3)

that is

$$\begin{aligned} \begin{array}{c} \begin{pmatrix} \dot{\phi } \\ \dot{\theta } \\ \dot{\psi } \end{pmatrix}= \begin{pmatrix} 1 &{} sin(\phi )tan(\theta ) &{} cos(\phi )tan(\theta ) \\ 0 &{} cos(\phi ) &{} -sin(\phi ) \\ 0 &{} sin(\phi )sec(\theta ) &{} cos(\phi )sec(\theta ) \end{pmatrix} \begin{pmatrix} p \\ q \\ r \end{pmatrix} \end{array} \end{aligned}$$

(9.4)

where $\eta =[\phi ,\theta ,\psi ]^T$ is the angular velocities vector in the inertial reference frame and $\omega =[p,q, r]^T$ is the angular velocities vector in the body-fixed reference frame.

9.2.3 Euler–Lagrange Equations for the Quadropter

The Euler–Lagrange equation for the quadropter is formulated as follows

$$\begin{aligned} \begin{array}{c} {{d} \over {dt}}({{\partial {L}} \over {{\partial }\dot{q}_i}})-{{{\partial }L} \over {{\partial }{q_i}}}= \begin{pmatrix} f_\xi \\ \tau _\eta \end{pmatrix} \end{array} \end{aligned}$$

(9.5)

where the Lagrangian is defined as $L(q,\dot{q})=E_{C_{trans}}+E_{C_{rot}}-E_p$, $E_{C_{trans}}$ is the kinetic energy of the quadrotor due to translational motion, $E_{C_{rot}}$ is the kinetic energy of the quadrotor due to rotational motion and $E_p$ is the total potential energy of the quadrotor due to lift. The generalized state vector is $q=[\xi ^T,\eta ^T]^T~{\in }~{R^6}$, ${\tau _\eta }~{\in }~{R^3}$ is the torques vector that causes rotation round the axes of the body-fixed reference frame, and $f_{\xi }=R\hat{f}+\alpha _T$ is the translational force applied to the quadropter due to the main control input $U_1$ along the z-axis direction, while $\alpha _T=[A_x,A_y, A_z]^T$ are the aerodynamic forces vector, defined along the axes of the inertial reference frame. Since the Lagrangian does not contain cross-coupling between the $\dot{\xi }$ and the $\dot{\eta }$ terms, the Lagrange–Euler equations can be divided into translational and rotational dynamics. The translational dynamics of the quadropter is given by

$$\begin{aligned} \begin{array}{c} m\ddot{\xi }+mg{e_3}=f_{\xi } \end{array} \end{aligned}$$

(9.6)

where $e_3=[0,0,1]^T$ is the unit vector along the z axis of the inertial reference frame. Eq. (9.6) can be written using the following three equations

$$\begin{aligned} \begin{array}{c} \ddot{x}={1 \over m}(cos(\psi )sin(\theta )cos(\phi )+sin(\psi )sin(\phi ))U_1+{A_x \over m} \\ \ddot{y}={1 \over m}(sin(\psi )sin(\theta )cos(\phi )-cos(\psi )sin(\phi ))U_1+{A_y \over m} \\ \ddot{z}=-g+{1 \over m}(cos(\theta )cos(\phi )){U_1}+{{A_z} \over m} \end{array} \end{aligned}$$

(9.7)

where m is the quadropter’s mass and g is the gravitational acceleration. The rotational dynamics of the quadropter is given by

$$\begin{aligned} \begin{array}{c} M(\eta )\ddot{\eta }+C(\eta ,\dot{\eta })\dot{\eta }={\tau _{\eta }} \end{array} \end{aligned}$$

(9.8)

where the inertia matrix $M(\eta )$ is defined as

$$\begin{aligned} \begin{array}{c} M(\eta )=\begin{pmatrix} I_{xx} &{} 0 &{} -{I_{xx}}S{\theta } \\ 0 &{} {I_{yy}}{C^2}\phi +{I_{zz}}{S^2}\phi &{} (I_{yy}-I_{zz})C{\phi }S{\phi }C{\theta } \\ -{I_{xx}}S{\theta } &{} (I_{yy}-I_{zz})C{\phi }S{\phi }C{\theta } &{} {I_{xx}}{S^2}{\theta }+{I_{yy}}{S^2}{\phi }{C^2}{\theta }+{I_{zz}}{C^2}{\phi }{C^2}{\theta } \end{pmatrix} \end{array} \end{aligned}$$

(9.9)

and the Coriolis matrix is

$$\begin{aligned} \begin{array}{c} C(\eta ,\dot{\eta })= \begin{pmatrix} c_{11} &{} c_{12} &{} c_{13} \\ c_{21} &{} c_{22} &{} c_{23} \\ c_{31} &{} c_{32} &{} c_{33} \end{pmatrix} \end{array} \end{aligned}$$

(9.10)

where the elements of the matrix are

$$\begin{aligned} \begin{array}{l} c_{11}=0 \\ c_{12}=({I_{yy}}-{I_{zz}})(\dot{\theta }C{\phi }S{\phi }+\dot{\psi }{S^2}{\phi }C{\theta })+({I_{zz}}-{I_{yy}})\dot{\psi }{C^2}{\phi }C{\theta }\\ c_{13}=({I_{zz}}-{I_{yy}})\dot{\psi }C{\phi }S{\phi }{C^2}\theta \\ c_{21}=({I_{zz}}-{I_{yy}})(\dot{\theta }C{\phi }S{\phi }+\dot{\psi }{S^2}{\phi }C{\theta })+(I_{yy}-I_{zz}){\dot{\psi }}{C^2}{\phi }C{\theta }+{I_{xx}}\dot{\psi }C{\theta } \\ c_{22}=({I_{zz}}-{I_{yy}}){\dot{\phi }}C{\phi }S{\phi } \\ c_{23}=-{I_{xx}}\dot{\psi }S{\theta }C{\theta }+{I_{yy}}\dot{\psi }{S^2}{\phi }C{\theta }S{\theta }+I_{zz}{\dot{\psi }}{C^2}{\phi }S{\theta }C{\theta } \\ c_{31}=({I_{yy}}-{I_{zz}})\dot{\psi }{C^2}{\theta }S{\phi }C{\phi }-{I_{xx}}{\dot{\theta }}C{\theta } \\ c_{32}=({I_{zz}}-{I_{yy}})(\dot{\theta }C{\phi }S{\phi }S{\theta }+\dot{\phi }{S^2}{\phi }C{\theta })+(I_{yy}-I_{zz})\dot{\phi }C^2{\phi }C{\theta }+{I_{xx}}\dot{\psi }S{\theta }C{\theta }- \\ -I_{yy}\dot{\psi }{S^2}{\phi }S{\theta }C{\theta }-I_{zz}\dot{\psi }C^2{\phi }S{\theta }C{\theta } \\ c_{33}=(I_{yy}-I_{zz})\dot{\phi }C{\phi }S{\phi }{C^2}\theta -{I_{yy}}\dot{\theta }{S^2}{\phi }C{\theta }S{\theta }-\\ -I_{zz}\dot{\theta }C^2{\phi }C{\theta }S{\theta }+I_{xx}\dot{\theta }C{\theta }S{\theta } \end{array} \end{aligned}$$

(9.11)

Thus, the mathematical model that describes the quadrotor’s rotational motion is given by

$$\begin{aligned} \begin{array}{c} \ddot{\eta }={M(\eta )^{-1}}(\tau _\eta -C(\eta ,\dot{\eta })\dot{\eta }) \end{array} \end{aligned}$$

(9.12)

Denoting $w={M(\eta )^{-1}}(\tau _\eta -C(\eta ,\dot{\eta })\dot{\eta })$, one has the following notation for the rotational dynamics

$$\begin{aligned} \begin{array}{c} \begin{pmatrix} \ddot{\phi } \\ \ddot{\theta } \\ \ddot{\psi } \end{pmatrix}= \begin{pmatrix} w_a\\ w_b\\ w_c \end{pmatrix} \end{array} \end{aligned}$$

(9.13)

Considering small variations of the heading angle of the quadrotor round $\psi ={\pi \over 2}$, denoting $w_1=U_1/m$ and taking also that the aerodynamic coefficients $A_x,A_y, A_z<<m$, a simplified quadropter’s model is formulated as follows [576]

$$\begin{aligned} \begin{array}{ccc} \ddot{x}={w_1}sin(\phi ) &{} \ddot{y}={w_1}cos(\phi )sin(\theta ) &{} \ddot{z}={w_1}cos(\phi )cos(\theta )-g \\ \ddot{\phi }=w_a &{} \ddot{\theta }=w_b &{} \ddot{\psi }=w_c \end{array}. \end{aligned}$$

(9.14)

9.2.4 Design of Flatness-Based Control for the Quadrotor’s Model

It will be shown, that the quadrotor’s model given in Eq. (9.14) is a differentially flat one, i.e. that all its state variables and the associated control inputs can be written as functions of a new variable called flat output and of its derivatives. The following state variables are introduced $x_1=x$, $x_2=\dot{x}$, $x_3=y$, $x_4=\dot{y}$, $x_5=z$, $x_6=\dot{z}$, $x_7=\phi$, $x_8=\dot{\phi }$, $x_9=\theta$, $x_{10}=\dot{\theta }$, $x_{11}=\psi$, $x_{12}=\dot{\psi }$. Thus, one has the following state-space description for the quadrotor’s dynamic model

$$\begin{aligned} \begin{array}{cccc} \dot{x}_1=x_2 &{} \dot{x}_2={w_1}sin(x_7) &{} \dot{x}_3=x_4 &{} \dot{x}_4={w_1}cos(x_7)sin(x_9) \\ \dot{x}_5=x_6 &{} \dot{x}_6={w_1}cos(x_7)cos(x_9) &{} \dot{x}_7=x_8 &{} \dot{x}_8=w_a \\ \dot{x}_9=x_{10} &{} \dot{x}_{10}=w_b &{} \dot{x}_{11}=x_{12} &{} \dot{x}_{12}=w_c \end{array} \end{aligned}$$

(9.15)

The flat output of the system is taken to be the vector $y_f=[x_1,x_3,x_5,x_7,x_9,x_{11}]^T$. It holds that

$$\begin{aligned} \begin{array}{cccc} x_1=[1 \ 0 \ 0 \ 0 \ 0 \ 0]y_f &{} x_2=[1 \ 0 \ 0 \ 0 \ 0 \ 0]\dot{y}_f &{} x_3=[0 \ 1 \ 0 \ 0 \ 0 \ 0]{y_f} &{} x_4=[0 \ 1 \ 0 \ 0 \ 0 \ 0]{\dot{y}_f} \\ x_5=[0 \ 0 \ 1 \ 0 \ 0 \ 0]y_f &{} x_6=[0 \ 0 \ 1 \ 0 \ 0 \ 0]\dot{y}_f &{} x_7=[0 \ 0 \ 0 \ 1 \ 0 \ 0]y_f &{}x_8=[0 \ 0 \ 0 \ 1 \ 0 \ 0]\dot{y}_f \\ x_9=[0 \ 0 \ 0 \ 0 \ 1 \ 0]y_f &{} x_{10}=[0 \ 0 \ 0 \ 0 \ 1 \ 0]\dot{y}_f &{}x_{11}=[0 \ 0 \ 0 \ 0 \ 0 \ 1]y_f &{} x_{12}=[0 \ 0 \ 0 \ 0 \ 0 \ 1]\dot{y}_f \end{array} \end{aligned}$$

(9.16)

According to Eq. (9.16) all state variables of the quadropter can be written as functions of the flat output and its derivatives. Using this and Eq. (9.15) one also has that the control inputs of the quadropter’s model, $w_1$, $w_a$, $w_b$ and $w_c$ can be written as functions of the flat output and its derivatives. Therefore, it is confirmed that the system is a differentially flat one. Defining now the new control inputs

$$\begin{aligned} \begin{array}{ccc} v_1={w_1}sin(x_7) &{} v_2={w_1}cos(x_7)sin(x_9) &{} v_3={w_1}cos(x_7)cos(x_9) \\ v_4=w_a &{} v_5=w_b &{}v_6=w_c \end{array} \end{aligned}$$

(9.17)

one has the following state-space description for the system

$$\begin{aligned} \begin{array}{c} \begin{pmatrix} \dot{y}_{f_1} \\ \ddot{y}_{f_1} \\ \dot{y}_{f_2} \\ \ddot{y}_{f_2} \\ \dot{y}_{f_3} \\ \ddot{y}_{f_3} \\ \dot{y}_{f_4} \\ \ddot{y}_{f_4} \\ \dot{y}_{f_5} \\ \ddot{y}_{f_5} \\ \dot{y}_{f_6} \\ \ddot{y}_{f_6} \\ \end{pmatrix}= \begin{pmatrix} 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 \ 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 \ 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 \ 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 \ 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 \ 1 \\ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \end{pmatrix} \begin{pmatrix} y_{f_1} \\ \dot{y}_{f_1} \\ y_{f_2} \\ \dot{y}_{f_2} \\ y_{f_3} \\ \dot{y}_{f_3} \\ y_{f_4} \\ \dot{y}_{f_4} \\ y_{f_5} \\ \dot{y}_{f_5} \\ y_{f_6} \\ \dot{y}_{f_6} \\ \end{pmatrix}+ \begin{pmatrix} 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 \\ 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 \end{pmatrix} \begin{pmatrix} v_1 \\ v_2 \\ v_3 \\ v_4 \\ v_5 \\ v_6 \end{pmatrix} \end{array} \end{aligned}$$

(9.18)

and the measurement equation for this system becomes

$$\begin{aligned} \begin{array}{c} \begin{pmatrix} z_1 \\ z_2 \\ z_3 \\ z_4 \\ z_5 \\ z_6 \\ \end{pmatrix}= \begin{pmatrix} 1 \ 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 \ 0 \ 1 \ 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 \ 0 \ 1 \ 0 \ 0 \ 0 \\ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 0 \ 1 \ 0 \\ \end{pmatrix} \begin{pmatrix} y_{f_1} \\ \dot{y}_{f_1} \\ y_{f_2} \\ \dot{y}_{f_2} \\ y_{f_3} \\ \dot{y}_{f_3} \\ y_{f_4} \\ \dot{y}_{f_4} \\ y_{f_5} \\ \dot{y}_{f_5} \\ y_{f_6} \\ \dot{y}_{f_6} \end{pmatrix} \end{array} \end{aligned}$$

(9.19)

Thus, using differential flatness theory the quadrotor’s model has been written in a MIMO linear canonical (Brunovsky) form, which is both controllable and observable. After being written in the linear canonical form the quadrotor’s state-space equation comprises 6 subsystems of the form

$$\begin{aligned} \begin{array}{c}\ddot{y}_{f_i}=v_i, \ i=1,\ldots , 6 \end{array} \end{aligned}$$

(9.20)

For each one of these subsystems a controller can be defined as follows

$$\begin{aligned} \begin{array}{c} v_i=\ddot{y}_{f_i}^d-{k_{d_i}}(\dot{y}_{f_i}-\dot{y}_{f_i}^d)-{k_{p_i}}(y_{f_i}-y_{f_i}^d), \ i=1,\ldots , 6 \end{array} \end{aligned}$$

(9.21)

The control scheme is implemented in the form of two cascading loops. The inner control loop controls rotation angles, while the outer control loop sets the desired values of the rotation angles so as to control position in the xyz-reference system. The computation of the reference setpoints for the rotation angles $\phi _d(t)$, $\theta _d(t)$ and $\psi _d(t)$ and for the cartesian coordinates $x_d(t)$, $y_d(t)$ and $z_d(t)$ takes into account the constraints imposed by the system dynamics.

9.2.5 Estimation of the Quadrotor’s Disturbance Forces and Torques with Kalman Filtering

It was shown that the initial nonlinear model of the quadrotor can be written in the MIMO canonical form of Eqs. (9.18) and (9.19). Next, it is assumed that the quadrotor’s model is affected by additive input disturbances, thus one has

$$\begin{aligned} \begin{array}{c} \ddot{x}_1=({w_1}+d_1)sin(x_7) \\ \ddot{x}_3=({w_1}+d_1)cos(x_7)sin(x_9) \\ \ddot{x}_5=({w_1}+d_1)cos(x_7)cos(x_9) \\ \ddot{x}_7={w_a}+{d_a} \\ \ddot{x}_9={w_b}+{d_b} \\ \ddot{x}_{11}={w_c}+{d_c} \end{array} \end{aligned}$$

(9.22)

or using the new state variables $y_{f_i} \ i=1,\ldots , 12$ of the differential flatness theory-based model and the transformed inputs $v_i, i=1,\ldots , 6$ one has

$$\begin{aligned} \begin{array}{c} \ddot{y}_{f_1}={v_1}+{d_1}sin(y_{f_7}) \\ \ddot{y}_{f_3}={v_2}+{d_1}cos(y_{f_7})sin(y_{f_9})\\ \ddot{y}_{f_5}={v_3}+{d_1}cos(y_{f_7})cos(y_{f_9}) \\ \ddot{y}_{f_7}={v_4}+{d_a} \\ \ddot{y}_{f_9}={v_5}+{d_b} \\ \ddot{y}_{f_{11}}={v_6}+{d_c} \end{array} \end{aligned}$$

(9.23)

while by redefining the disturbance terms as $\tilde{d}_1={d_1}sin(y_{f_7})$, $\tilde{d}_2={d_1}cos(y_{f_7})sin(y_{f_9})$, $\tilde{d}_3={d_1}cos(y_{f_7})cos(y_{f_9})$, $\tilde{d}_4=d_a$, $\tilde{d}_5=d_b$ and $\tilde{d}_6=d_c$, the dynamics of the disturbed system can be written as

$$\begin{aligned} \begin{array}{cc} \ddot{y}_{f_1}=v_1+\tilde{d}_1 &{} \ddot{y}_{f_3}=v_2+\tilde{d}_2 \\ \ddot{y}_{f_5}=v_3+\tilde{d}_3 &{} \ddot{y}_{f_7}=v_4+\tilde{d}_4 \\ \ddot{y}_{f_9}=v_5+\tilde{d}_5 &{} \ddot{y}_{f_{11}}=v_6+\tilde{d}_6 \end{array} \end{aligned}$$

(9.24)

The system’s dynamics can be also written as $\dot{y}_{f_1}=y_{f_2}$, $\dot{y}_{f_2}={v_1}+\tilde{d}_1$, $\dot{y}_{f_3}=y_{f_4}$, $\dot{y}_{f_4}={v_2}+\tilde{d}_2$, $\dot{y}_{f_5}=y_{f_6}$, $\dot{y}_{f_6}={v_3}+\tilde{d}_3$, $\dot{y}_{f_7}=y_{f_8}$, $\dot{y}_{f_8}={v_4}+\tilde{d}_4$, $\dot{y}_{f_9}=y_{f_{10}}$, $\dot{y}_{f_{10}}={v_5}+\tilde{d}_5$, $\dot{y}_{f_{11}}=y_{f_6}$, $\dot{y}_{f_6}={v_6}+\tilde{d}_6$.

Without loss of generality, it is assumed that the dynamics of the disturbances terms are described by their second order derivative, i.e. $\ddot{\tilde{d}}_i=f_{d_i}, \ i=1,\ldots , 6$. Next, the extended state vector of the system is defined so as to include disturbance terms as well. Thus one has the following state variables

$$\begin{aligned} \begin{array}{cccccc} z_{f_1}=y_{f_1} &{} z_{f_2}=y_{f_2} &{} z_{f_3}=y_{f_3} &{} z_{f_4}=y_{f_4} &{} z_{f_5}=y_{f_5} &{} z_{f_6}=y_{f_6} \\ z_{f_7}=y_{f_7} &{} z_{f_8}=y_{f_8} &{} z_{f_9}=y_{f_9} &{}z_{f_{10}}=y_{f_{10}} &{}z_{f_{11}}=y_{f_{11}} &{}z_{f_{12}}=y_{f_{12}} \\ z_{f_{13}}=\tilde{d}_1 &{} z_{f_{14}}=\dot{\tilde{d}}_1 &{} z_{f_{15}}=\ddot{\tilde{d}}_1 &{} z_{f_{16}}=\tilde{d}_2 &{} z_{f_{17}}=\dot{\tilde{d}}_2 &{} z_{f_{18}}=\ddot{\tilde{d}}_2 \\ z_{f_{19}}=\tilde{d}_3 &{} z_{f_{20}}=\dot{\tilde{d}}_3 &{} z_{f_{21}}=\ddot{\tilde{d}}_3 &{} z_{f_{22}}=\tilde{d}_4 &{}z_{f_{23}}=\dot{\tilde{d}}_4 &{} z_{f_{24}}=\ddot{\tilde{d}}_4 \\ z_{f_{25}}=\tilde{d}_5 &{} z_{f_{26}}=\dot{\tilde{d}}_6 &{} z_{f_{27}}=\ddot{\tilde{d}}_5 &{} z_{f_{28}}=\tilde{d}_6 &{}z_{f_{29}}=\dot{\tilde{d}}_6 &{}z_{f_{30}}=\ddot{\tilde{d}}_6 \end{array} \end{aligned}$$

(9.25)

Thus, the disturbed system can be described by a state-space equation of the form

$$\begin{aligned} \begin{array}{c} \dot{z}_f={A_f}{z_f}+{B_f}v \\ z_{f}^{meas}={C_f}{z_f} \end{array} \end{aligned}$$

(9.26)

where ${A_f}~{\in }~R^{{30}\times {30}}$, ${B_f}~{\in }~R^{{30}\times {6}}$ and ${C_f}~{\in }~R^{{6}\times {30}}$, with

$$\begin{aligned} \begin{array}{ccc} A_f=\begin{pmatrix} 0_{1{\times }1} &{} 1 &{} 0_{1{\times }28} \\ 0_{1{\times }12} &{} 1 &{} 0_{1{\times }17} \\ 0_{1{\times }3} &{} 1 &{} 0_{1{\times }26} \\ 0_{1{\times }15} &{} 1 &{} 0_{1{\times }14} \\ 0_{1{\times }5} &{} 1 &{} 0_{1{\times }24} \\ 0_{1{\times }18} &{} 1 &{} 0_{1{\times }11} \\ 0_{1{\times }7} &{} 1 &{} 0_{1{\times }22} \\ 0_{1{\times }21} &{} 1 &{} 0_{1{\times }8} \\ 0_{1{\times }9} &{} 1 &{} 0_{1{\times }20} \\ 0_{1{\times }24} &{} 1 &{} 0_{1{\times }5} \\ 0_{1{\times }11} &{} 1 &{} 0_{1{\times }18} \\ 0_{1{\times }27} &{} 1 &{} 0_{1{\times }2} \\ 0_{1{\times }13} &{} 1 &{} 0_{1{\times }16} \\ 0_{1{\times }14} &{} 1 &{} 0_{1{\times }15} \\ 0_{1{\times }30} &{} &{} \\ 0_{1{\times }16} &{} 1 &{} 0_{1{\times }13} \\ 0_{1{\times }17} &{} 1 &{} 0_{1{\times }12} \\ 0_{1{\times }30} &{} &{} \\ 0_{1{\times }19} &{} 1 &{} 0_{1{\times }10} \\ 0_{1{\times }20} &{} 1 &{} 0_{1{\times }9} \\ 0_{1{\times }30} &{} &{} \\ 0_{1{\times }22} &{} 1 &{} 0_{1{\times }7} \\ 0_{1{\times }23} &{} 1 &{} 0_{1{\times }6} \\ 0_{1{\times }30} &{} &{} \\ 0_{1{\times }25} &{} 1 &{} 0_{1{\times }4} \\ 0_{1{\times }26} &{} 1 &{} 0_{1{\times }3} \\ 0_{1{\times }30} &{} &{} \\ 0_{1{\times }28} &{} 1 &{} 0_{1{\times }1} \\ 0_{1{\times }29} &{} 1 &{} \\ 0_{1{\times }30} &{} &{} \\ \end{pmatrix} &{} B_f=\begin{pmatrix} 0_{1{\times }6} &{} &{} \\ 1 &{} 0_{1{\times }5} \\ 0_{1{\times }1} &{} 1 &{} 0_{1{\times }4} \\ 0_{1{\times }6} &{} &{} \\ 0_{1{\times }2} &{} 1 &{} 0_{1{\times }3} \\ 0_{1{\times }6} &{} &{} \\ 0_{1{\times }3} &{} 1 &{} 0_{1{\times }2} \\ 0_{1{\times }6} &{} &{} \\ 0_{1{\times }4} &{} 1 &{} 0_{1{\times }1} \\ 0_{1{\times }6} &{} &{} \\ 0_{1{\times }5} &{} 1 &{} \\ 0_{18{\times }6} \end{pmatrix} &{} C_f=\begin{pmatrix} 1 &{} 0_{1{\times }29} &{} \\ 0_{1{\times }2} &{} 1 &{} 0_{1{\times }27} &{} \\ 0_{1{\times }4} &{} 1 &{} 0_{1{\times }25} &{} \\ 0_{1{\times }6} &{} 1 &{} 0_{1{\times }23} &{} \\ 0_{1{\times }8} &{} 1 &{} 0_{1{\times }21} &{} \\ 0_{1{\times }10} &{} 1 &{} 0_{1{\times }19} &{} \\ \end{pmatrix} \end{array} \end{aligned}$$

(9.27)

For the aforementioned model, and after carrying out discretization of matrices $A_f$, $B_f$ and $C_f$ with common discretization methods one can implement the standard Kalman Filter algorithm using Eqs. (9.29) and (9.30). This is Derivative-free nonlinear Kalman filtering for the model of the quadropter which, unlike EKF, is performed without the need to compute Jacobian matrices and does not introduce numerical errors due to approximate linearization with Taylor series expansion.

The dynamics of the disturbance terms $\tilde{d}_i, \ i=1,\ldots , 6$ are taken to be unknown in the design of the associated disturbances’ estimator. Defining as $\tilde{A}_d$, $\tilde{B}_d$, and $\tilde{C}_d$, the discrete-time equivalents of matrices $\tilde{A}_f$, $\tilde{B}_f$ and $\tilde{C}_f$ respectively, one has the following dynamics:

$$\begin{aligned} \dot{\hat{z}}_f=\tilde{A}_f{\cdot }{\hat{z}_f}+\tilde{B}_f{\cdot }{\tilde{v}}+K(z_f^{meas}-{\tilde{C}_f}{\hat{z}_f}) \end{aligned}$$

(9.28)

where $K~{\in }~R^{30{\times }6}$ is the state estimator’s gain. The associated Kalman Filter-based disturbance estimator is given by [439, 445]

Measurement update:

$$\begin{aligned} \begin{array}{l} K(k)={P^{-}(k)}{\tilde{C}_d^T}[\tilde{C}_d{\cdot }P^{-}(k){\tilde{C}_d^T}+R]^{-1} \\ {\hat{z}_f(k)}={\hat{z}_f^{-}(k)}+K(k)[z_f^{meas}(k)-\tilde{C}_d{\hat{z}_f^{-}(k)}] \\ P(k)=P^{-}(k)-K(k){\tilde{C}_d}P^{-}(k) \end{array} \end{aligned}$$

(9.29)

Time update:

$$\begin{aligned} \begin{array}{l} P^{-}(k+1)={\tilde{A}_d(k)}P(k){\tilde{A}_d^T}(k)+Q(k) \\ {\hat{z}_f^{-}(k+1)}=\tilde{A}_d(k)\hat{z}_f(k)+\tilde{B}_d(k)\tilde{v}(k) \end{array} \end{aligned}$$

(9.30)

To compensate for the effects of the disturbance forces it suffices to use in the control loop the modified control input vector

$$\begin{aligned} \begin{array}{c} v=\begin{pmatrix} v_1-\hat{\tilde{d}}_1 \\ v_2-\hat{\tilde{d}}_2 \\ v_3-\hat{\tilde{d}}_3 \\ v_4-\hat{\tilde{d}}_4 \\ v_5-\hat{\tilde{d}}_5 \\ v_6-\hat{\tilde{d}}_6 \end{pmatrix} \ \text {or} \ v=\begin{pmatrix} v_1-\hat{z}_{13} \\ v_2-\hat{z}_{16} \\ v_3-\hat{z}_{19} \\ v_4-\hat{z}_{22} \\ v_5-\hat{z}_{25} \\ v_6-\hat{z}_{28} \end{pmatrix} \end{array}. \end{aligned}$$

(9.31)

9.2.6 Simulation Tests

Initial simulation experiments were concerned with flight control of the quadropter in the disturbance-free case. The considered reference trajectories are shown in Fig. 9.2. The implementation of the flatness-based control enabled accurate tracking of the reference trajectories. Convergence has been achieved for the linear position and velocity variables to the associated setpoints as it can be seen in Figs. 9.3a, b and 9.4a. Moreover, there has been convergence of the angular position and velocity variables to the associated setpoints as it can be seen in Figs. 9.4a and 9.5a, b.

Fig. 9.2

Control of the quadrotor in the disturbance free-case: a trajectory of the quadrotor in the cartesian space, b projection of the quadrotor’s trajectory in the xy plane

Fig. 9.3

Control of the quadrotor in the disturbance free-case: a position and velocity along the x axis, b position and velocity along the y axis

Fig. 9.4

Control of the quadrotor in the disturbance free-case: a position and velocity along the z axis, b rotation angle $\phi$ and associated angular speed

Fig. 9.5

Control of the quadrotor in the disturbance free-case: a rotation angle $\theta$ and associated angular speed, b rotation angle $\psi$ and associated angular speed

Additional simulation experiments were concerned with control of the quadropter in flight under disturbance forces and torques. The estimation of the disturbance forces and torques is shown in Fig. 9.6. The implementation of the flatness-based control enabled accurate tracking of the reference trajectories. There has been convergence of the linear position and velocity variables to the associated setpoints as it can be seen in Figs. 9.7a, b and 9.8a. Moreover, there has been convergence of the angular position and velocity variables to the associated setpoints as it can be seen in Figs. 9.8b and 9.9a, b.

Fig. 9.6

Use of the Derivative-free nonlinear Kalman Filter in estimation of disturbances: a associated with linear motion, b associated with the rotational motion of the vehicle (blue line: real value, green line estimated value)

Fig. 9.7

Control of the quadrotor in the presence of external disturbances a position and velocity along the x axis, b position and velocity along the y axis (blue line: real value, green line estimated value, red line: setpoint)

Fig. 9.8

Control of the quadrotor in the presence of external disturbances: a position and velocity along the z axis, b rotation angle $\phi$ and associated angular speed (blue line: real value, green line estimated value, red line: setpoint)

Fig. 9.9

Control of the quadrotor in the presence of disturbances: a rotation angle $\theta$ and associated angular speed, b rotation angle $\psi$ and associated angular speed (blue line: real value, green line estimated value, red line: setpoint)

9.3 Control of UAVs Based on Approximate Linearization

9.3.1 Outline

The present section analyzes a nonlinear control method for unmanned aerial vehicles (UAVs) which is based on local linearization of the UAVs dynamics and on application of H-infinity control theory. As previously mentioned, the complete 6-DOF dynamic model of the UAV is a highly nonlinear one and its control can be performed with (i) global linearization control methods [6, 77, 152, 430, 438, 452, 457, 576], (ii) local linearization control methods [17, 55, 131, 212, 264, 401, 403, 450, 461, 480, 587] and (iii) Lyapunov analysis-based methods [13, 48, 60, 122, 135, 193, 261, 653]. In approach (i) the dynamic model of the UAV is transformed into an equivalent linear description through the application of a change of variables (diffeomorphisms). In (ii) the nonlinear model of the UAV is decomposed into local linear models for which linear feedback controllers are designed and next the aim is to select the feedback control gains so as to assure the global asymptotic stability of the control loop. In (iii) the objective is to define an energy function for the UAV (Lyapunov function) and to demonstrate that through suitable selection of the feedback control the first derivative of the energy function is always negative and thus the global stability of the control loop is assured. The latter approach is particularly suitable for model-free control of UAVs as in the case of adaptive control methods.

In this section the control of the UAV makes use of an approach of local linearization. The linearization takes place round the UAV’s local operating point which is defined at each time instant by the present value of the state vector and the last value of the control inputs vector [461]. The linearization is based on Taylor series expansion and on the computation of the associated Jacobian matrices. The modelling error, due to truncation of higher order terms in the Taylor series, is considered as perturbation which is compensated by the robustness of the control algorithm. For the linearized model of the UAV an H-infinity feedback controller is designed. A cost function is introduced comprising the weighted square of the error of the system’s state vector (distance of the state vector from the reference setpoints).

As explained in previous applications of H-infinity control, this control scheme represents a differential game taking place between the control input which tries to minimize the above cost function and between the disturbances which try to maximize this objective function. The computation of the feedback control gain relies on the solution of an algebraic Riccati equation, which is performed at each iteration of the control algorithm. The solution of the Riccati equation provides a positive definite symmetric matrix which is used as a weighting coefficient in the computation of the controller’s feedback gain. The known robustness features of H-infinity control assure the elimination of perturbation effects, which implies compensation of model uncertainty terms, external disturbance inputs as well as of measurement noises. The stability properties of the control scheme are assured with the use of Lyapunov analysis. First, it is shown that the proposed UAV feedback control law results in H-infinity tracking performance which means robustness against modeling uncertainty and external perturbations. Under moderate conditions it is also shown that the control loop is also globally asymptotically stable. The tracking accuracy and the smooth transients in the proposed UAV control method are also confirmed through simulation experiments.

Comparing to nonlinear feedback control approaches which rely on exact feedback linearization of unmanned aerial vehicles (as the ones based on differential flatness theory [452, 457]) the proposed $H_{\infty }$ control scheme is assessed as follows: (i) it uses an approximate linearization of the system’s dynamic model which does not follow the elaborated transformations (diffeomorphisms) of the exact linearization methods, (ii) the method is applied directly on the initial nonlinear model of the UAV, and does not inverse transformations. In this manner it is unlikely to come against singularities in the computation of the UAV’s real control inputs, (iii) the method retains the advantages of optimal control techniques, that is the best trade-off between setpoint tracking and moderate variations of the control inputs.

9.3.2 Dynamic Model of the UAV

9.3.2.1 State-Space Description of the UAV

By following the previous analysis of the dynamic model of the UAV, given in Sect. 9.2.3 and by considering that $w_a={1 \over J_x}{\tau _\phi }$, $w_b={1 \over J_y}{\tau _\theta }$, and $w_c={1 \over J_z}{\tau _\psi }$, the following dynamic model of the UAV is obtained:

$$\begin{aligned} \begin{array}{c} \ddot{x}={1 \over m}[cos(\psi )sin(\theta )cos(\phi )+sin(\psi )sin(\phi )]{U_1}+{A_x \over m} \\ \ddot{y}={1 \over m}[sin(\psi )sin(\theta )cos(\phi )-cos(\psi )sin(\phi )]{U_1}+{A_y \over m} \\ \ddot{z}={1 \over m}[cos(\theta )cos(\phi )]{U_1}+{A_z \over m}-g \\ \ddot{\phi }={1 \over J_x}{\tau _\phi } \\ \ddot{\theta }={1 \over J_y}{\tau _\theta } \\ \ddot{\psi }={1 \over J_z}{\tau _\psi } \end{array} \end{aligned}$$

(9.32)

where x, y, z are the coordinates of the UAV’s center of gravity in a cartesian reference frame, $\phi$, $\theta$, $\psi$ are the Euler rotation angles describing roll, pitch and yaw motion respectively, m is the UAV’s mass, $J_x$, $J_y$, $J_z$ are the UAV’s moments of inertia for rotation round the cartesian coordinates axes $A_x$, $A_y$, $A_z$ are aerodynamic forces exerted on the UAV for motion along the cartesian axes and g is the acceleration of gravity.

Next, the UAV’s dynamic model is written in state-space form by defining the following state variables: $x_1=x$, $x_2=\dot{x}$, $x_3=y$, $x_4=\dot{y}$, $x_5=z$, $x_6=\dot{z}$, $x_7=\phi$, $x_8=\dot{\phi }$, $x_9=\theta$, $x_{10}=\dot{\theta }$, $x_{11}=\psi$, $x_{12}=\dot{\psi }$. Thus, one obtains:

$$\begin{aligned} \begin{array}{c} \dot{x}_1=x_2 \\ \dot{x}_2={1 \over m}[cos(\psi )sin(\theta )cos(\phi )+sin(\psi )sin(\phi )]{U_1}+{A_x \over m} \\ \dot{x}_3=x_4 \\ \dot{x}_4={1 \over m}[sin(\psi )sin(\theta )cos(\phi )-cos(\psi )sin(\phi )]{U_1}+{A_y \over m} \\ \dot{x}_5=x_6 \\ \dot{x}_6={1 \over m}[cos(\theta )cos(\phi )]U_1+{A_z \over m}-g \\ \dot{x}_7=x_8 \\ \dot{x}_8={1 \over J_x}{\tau _\phi } \\ \dot{x}_9=x_{10} \\ \dot{x}_{10}={1 \over J_y}{\tau _\theta } \\ \dot{x}_{11}=x_{12} \\ \dot{x}_{12}={1 \over J_z}{\tau _\psi } \end{array} \end{aligned}$$

(9.33)

or equivalently

$$\begin{aligned} \begin{array}{c} \dot{x}=f(x, u) \\ \end{array} \end{aligned}$$

(9.34)

which is analytically written as

$$\begin{aligned} \begin{array}{c} \begin{pmatrix} \dot{x}_1 \\ \dot{x}_2 \\ \dot{x}_3 \\ \dot{x}_4 \\ \dot{x}_5 \\ \dot{x}_6 \\ \dot{x}_7 \\ \dot{x}_8 \\ \dot{x}_9 \\ \dot{x}_{10} \\ \dot{x}_{11} \\ \dot{x}_{12} \end{pmatrix}= \begin{pmatrix} x_2 \\ {1 \over m}[cos(\psi )sin(\theta )cos(\phi )+sin(\psi )sin(\phi )]{u_1}+{A_x \over m} \\ x_4 \\ {1 \over m}[sin(\psi )sin(\theta )cos(\phi )-cos(\psi )sin(\phi )]{u_1}+{A_y \over m} \\ x_6 \\ {1 \over m}[cos(\theta )cos(\phi )]u_1+{A_z \over m}-g \\ x_8 \\ {1 \over J_x}{\tau _\phi } \\ x_{10} \\ {1 \over J_y}{\tau _\theta } \\ x_{11} \\ {1 \over J_z}{\tau _\psi } \end{pmatrix} \\ \end{array} \end{aligned}$$

(9.35)

where the input vector u is defined as $u=[u_1,u_2,u_3,u_4]^T=[U_1,{\tau _\phi },{\tau _\theta },{\tau _\psi }]^T$.

9.3.3 Linearization of the UAV’s Dynamic Model

Linearization of the previous dynamic model of the UAV $\dot{x}=f(x, u)$ can be performed round local operating points $(x^{*}, u^{*})$, where $x^{*}$ is the present value of the UAV’s state vector and $u^{*}$ is the last value of the control input which has been exerted on the system. By applying Taylor series expansion one obtains

$$\begin{aligned} \begin{array}{c} \dot{x}={{J_x}f}\mid _{(x^{*},u^{*})}x+{{J_u}f}\mid _{(x^{*}, u^{*})}u+\tilde{d} \end{array} \end{aligned}$$

(9.36)

or equivalently

$$\begin{aligned} \begin{array}{c} \dot{x}=Ax+Bu+\tilde{d} \end{array} \end{aligned}$$

(9.37)

where $A={{J_x}f}\mid _{(x^{*}, u^{*})}$, $B={{J_u}f}\mid _{(x^{*}, u^{*})}$ and $\tilde{d}$ is the modelling error due to the truncation of higher order terms in the Taylor series expansion. Next, the Jacobian matrices of the UAV’s dynamic model with respect to its state variables are computed

$$\begin{aligned} \begin{array}{c} {J_x}f= \begin{pmatrix} {{{\partial }{f_1}} \over {{\partial }{x_1}}} &{} {{{\partial }{f_1}} \over {{\partial }{x_2}}} &{} \cdots &{} \cdots &{} {{{\partial }{f_1}} \over {{\partial }{x_n}}} \\ {{{\partial }{f_2}} \over {{\partial }{x_1}}} &{} {{{\partial }{f_2}} \over {{\partial }{x_2}}} &{} \cdots &{} \cdots &{} {{{\partial }{f_2}} \over {{\partial }{x_{12}}}} \\ \cdots &{} \cdots &{} \cdots &{} \cdots &{} \cdots \\ \cdots &{} \cdots &{} \cdots &{} \cdots &{} \cdots \\ {{{\partial }{f_{11}}} \over {{\partial }{x_1}}} &{} {{{\partial }{f_{11}}} \over {{\partial }{x_2}}} &{} \cdots &{} \cdots &{} {{{\partial }{f_{11}}} \over {{\partial }{x_{12}}}} \\ {{{\partial }{f_{12}}} \over {{\partial }{x_1}}} &{} {{{\partial }{f_{12}}} \over {{\partial }{x_2}}} &{} \cdots &{} \cdots &{} {{{\partial }{f_{12}}} \over {{\partial }{x_{12}}}} \end{pmatrix} \end{array} \end{aligned}$$

(9.38)

It holds that the 1st row of the Jacobian matrix ${J_x}f$ is

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_1}} \over {{\partial }{x_1}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_2}}}=1 &{} {{{\partial }{f_1}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_1}} \over {{\partial }{x_5}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_6}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_7}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_1}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_{10}}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_{11}}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.39)

2nd row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{c} {{{\partial }{f_2}} \over {{\partial }{x_1}}}=0 \ \ {{{\partial }{f_2}} \over {{\partial }{x_2}}}=0 \ \ {{{\partial }{f_2}} \over {{\partial }{x_3}}}=0 \\ {{{\partial }{f_2}} \over {{\partial }{x_4}}}=0 \ \ {{{\partial }{f_2}} \over {{\partial }{x_5}}}=0 \ \ {{{\partial }{f_2}} \over {{\partial }{x_6}}}=0 \\ {{{\partial }{f_2}} \over {{\partial }{x_7}}}={1 \over m}[-cos(x_{11})sin(x_9)sin(x_7)-sin(x_{11}sin(x_7))]u_1 \ \ {{{\partial }{f_2}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_2}} \over {{\partial }{x_9}}}={1 \over m}[cos(x_{11})cos(x_9)cos(x_7)]u_1 \ \ {{{\partial }{f_2}} \over {{\partial }{x_{10}}}}=0 \\ {{{\partial }{f_2}} \over {{\partial }{x_{11}}}}={1 \over m}[-sin(x_{11})sin(x_9)cos(x_7)+cos(x_{11})cos(x_7)]u_1 \ \ {{{\partial }{f_2}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.40)

3rd row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_1}} \over {{\partial }{x_1}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_2}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_3}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_4}}}=1 \\ {{{\partial }{f_1}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_6}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_1}} \over {{\partial }{x_9}}}=0 &{}{{{\partial }{f_1}} \over {{\partial }{x_{10}}}}=0 \\ {{{\partial }{f_1}} \over {{\partial }{x_{11}}}}=0 &{} {{{\partial }{f_1}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.41)

4th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{c} {{{\partial }{f_4}} \over {{\partial }{x_1}}}=0 \ \ {{{\partial }{f_4}} \over {{\partial }{x_2}}}=0 \ \ {{{\partial }{f_4}} \over {{\partial }{x_3}}}=0 \\ {{{\partial }{f_4}} \over {{\partial }{x_4}}}=0 \ \ {{{\partial }{f_4}} \over {{\partial }{x_5}}}=0 \ \ {{{\partial }{f_4}} \over {{\partial }{x_6}}}=0 \\ {{{\partial }{f_4}} \over {{\partial }{x_7}}}={1 \over m}[sin(x_{11})sin(x_9)cos(x_7)-cos(x_{11}cos(x_7))]u_1 \\ {{{\partial }{f_4}} \over {{\partial }{x_8}}}=0 \ \ {{{\partial }{f_4}} \over {{\partial }{x_9}}}={1 \over m}[sin(x_{11})cos(x_9)cos(x_7)]u_1 \\ {{{\partial }{f_4}} \over {{\partial }{x_{10}}}}=0 \ \ {{{\partial }{f_4}} \over {{\partial }{x_{11}}}}={1 \over m}[cos(x_{11})sin(x_9)sin(x_7)+sin(x_{11})sin(x_7)]u_1 \ \ {{{\partial }{f_4}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.42)

5th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_5}} \over {{\partial }{x_1}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_2}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_5}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_6}}}=1 &{} {{{\partial }{f_5}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_5}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_{10}}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_{11}}}}=0 &{} {{{\partial }{f_5}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.43)

6th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{c} {{{\partial }{f_6}} \over {{\partial }{x_1}}}=0 \ \ {{{\partial }{f_6}} \over {{\partial }{x_2}}}=0 \ \ {{{\partial }{f_6}} \over {{\partial }{x_3}}}=0 \\ {{{\partial }{f_6}} \over {{\partial }{x_4}}}=0 \ \ {{{\partial }{f_6}} \over {{\partial }{x_5}}}=0 \ \ {{{\partial }{f_6}} \over {{\partial }{x_6}}}=0 \\ {{{\partial }{f_6}} \over {{\partial }{x_7}}}={1 \over m}[-cos(x_{9})sin(x_7)]u_1 \ \ {{{\partial }{f_6}} \over {{\partial }{x_8}}}=0 \ \ {{{\partial }{f_6}} \over {{\partial }{x_9}}}={1 \over m}[-sin(x_9)cos(x_7)]u_1 \\ {{{\partial }{f_6}} \over {{\partial }{x_{10}}}}=0 \ \ {{{\partial }{f_6}} \over {{\partial }{x_{11}}}}=0 \ \ {{{\partial }{f_6}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.44)

7th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_7}} \over {{\partial }{x_1}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_2}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_7}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_6}}}=0 &{}{{{\partial }{f_7}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_8}}}=1 \\ {{{\partial }{f_7}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_{10}}}}=0 &{}{{{\partial }{f_7}} \over {{\partial }{x_{11}}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.45)

8th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_7}} \over {{\partial }{x_1}}}=0 &{}{{{\partial }{f_7}} \over {{\partial }{x_2}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_7}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_6}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_7}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_{10}}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_{11}}}}=0 &{} {{{\partial }{f_7}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.46)

9th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_9}} \over {{\partial }{x_1}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_2}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_9}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_6}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_9}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_{10}}}}=1 &{} {{{\partial }{f_9}} \over {{\partial }{x_{11}}}}=0 &{} {{{\partial }{f_9}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.47)

10th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{10}}} \over {{\partial }{x_1}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_2}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_{10}}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_6}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_{10}}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_{10}}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_{11}}}}=0 &{} {{{\partial }{f_{10}}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.48)

11th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{11}}} \over {{\partial }{x_1}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_2}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_{11}}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_6}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_{11}}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_{10}}}}=0 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_{11}}}}=1 &{} {{{\partial }{f_{11}}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.49)

12th row of the Jacobian matrix ${J_x}f$

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{12}}} \over {{\partial }{x_1}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_2}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_3}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_4}}}=0 \\ {{{\partial }{f_{12}}} \over {{\partial }{x_5}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_6}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_7}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_8}}}=0 \\ {{{\partial }{f_{12}}} \over {{\partial }{x_9}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_{10}}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_{11}}}}=0 &{} {{{\partial }{f_{12}}} \over {{\partial }{x_{12}}}}=0 \end{array} \end{aligned}$$

(9.50)

Next, the Jacobian matrix of the UAV is computed with respect to the elements of the control input vector $U=[u_1,u_2,u_3,u_4]^T=[U_1,\tau _\phi ,\tau _\theta ,\tau _\psi ]^T$. It holds that

$$\begin{aligned} \begin{array}{c} {J_u}f= \begin{pmatrix} {{{\partial }{f_1}} \over {{\partial }{u_1}}} &{} {{{\partial }{f_1}} \over {{\partial }{u_2}}} &{} {{{\partial }{f_1}} \over {{\partial }{u_3}}} &{} {{{\partial }{f_1}} \over {{\partial }{u_4}}} \\ {{{\partial }{f_2}} \over {{\partial }{u_1}}} &{} {{{\partial }{f_2}} \over {{\partial }{u_2}}} &{} {{{\partial }{f_2}} \over {{\partial }{u_3}}} &{} {{{\partial }{f_2}} \over {{\partial }{u_4}}} \\ \cdots &{} \cdots &{} \cdots &{} \cdots \\ \cdots &{} \cdots &{} \cdots &{} \cdots \\ {{{\partial }{f_{11}}} \over {{\partial }{u_1}}} &{} {{{\partial }{f_{11}}} \over {{\partial }{u_2}}} &{} {{{\partial }{f_{11}}} \over {{\partial }{u_3}}} &{} {{{\partial }{f_{11}}} \over {{\partial }{u_4}}} \\ {{{\partial }{f_{12}}} \over {{\partial }{u_1}}} &{} {{{\partial }{f_{12}}} \over {{\partial }{u_2}}} &{} {{{\partial }{f_{12}}} \over {{\partial }{u_3}}} &{} {{{\partial }{f_{12}}} \over {{\partial }{u_4}}} \\ \end{pmatrix} \end{array} \end{aligned}$$

(9.51)

About the 1st row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{1}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{1}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{1}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{1}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.52)

About the 2nd row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cc} {{{\partial }{f_{2}}} \over {{\partial }{u_1}}}={1 \over m}[cos(x_{11})sin((x_9)cos(x_7)+sin(x_{11})cos(x_7)] &{} {{{\partial }{f_{2}}} \over {{\partial }{u_2}}}=0 \\ {{{\partial }{f_{2}}} \over {{\partial }{u_3}}}=0 &{} {{{\partial }{f_{2}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.53)

About the 3rd row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{1}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{1}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{1}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{1}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.54)

About the 4th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cc} {{{\partial }{f_{4}}} \over {{\partial }{u_1}}}={1 \over m}[sin(x_{11})sin((x_9)sin(x_7)-cos(x_{11})sin(x_7)] &{} {{{\partial }{f_{4}}} \over {{\partial }{u_2}}}=0 \\ {{{\partial }{f_{4}}} \over {{\partial }{u_3}}}=0 &{} {{{\partial }{f_{4}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.55)

About the 5th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{5}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{5}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{5}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{5}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.56)

About the 6th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cc} {{{\partial }{f_{6}}} \over {{\partial }{u_1}}}={1 \over m}[cos((x_9)cos(x_7)] &{} {{{\partial }{f_{6}}} \over {{\partial }{u_2}}}=0 \\ {{{\partial }{f_{6}}} \over {{\partial }{u_3}}}=0 &{} {{{\partial }{f_{6}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.57)

About the 7th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{7}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{7}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{7}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{7}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.58)

About the 8th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{8}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{8}}} \over {{\partial }{u_2}}}={1 \over J_x}&{{{\partial }{f_{8}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{8}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.59)

About the 9th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{9}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{9}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{9}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{9}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.60)

About the 10th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{10}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{10}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{10}}} \over {{\partial }{u_3}}}={1 \over J_y}&{{{\partial }{f_{10}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.61)

About the 11th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{11}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{11}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{11}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{11}}} \over {{\partial }{u_4}}}=0 \end{array} \end{aligned}$$

(9.62)

About the 12th row of the Jacobian matrix ${J_u}f$ it holds

$$\begin{aligned} \begin{array}{cccc} {{{\partial }{f_{12}}} \over {{\partial }{u_1}}}=0&{{{\partial }{f_{12}}} \over {{\partial }{u_2}}}=0&{{{\partial }{f_{12}}} \over {{\partial }{u_3}}}=0&{{{\partial }{f_{12}}} \over {{\partial }{u_4}}}={1 \over J_z} \end{array} \end{aligned}$$

(9.63)

9.3.4 Design of an H-Infinity Nonlinear Feedback Controller

9.3.4.1 Equivalent Linearized Dynamics of the Robot

After linearization around its current operating point, the UAV’s dynamic model is written as

$$\begin{aligned} \begin{array}{c} \dot{x}=Ax+Bu+d_1 \end{array} \end{aligned}$$

(9.64)

Parameter $d_1$ stands for the linearization error in the UAV’s dynamic model appearing in Eq. (9.64). The reference setpoints for the UAV’s state vector are denoted by $\mathbf{{x_d}}=[x_1^{d},\ldots , x_4^{d}]$. Tracking of this trajectory is achieved after applying the control input $u^{*}$. At every time instant the control input $u^{*}$ is assumed to differ from the control input u appearing in Eq. (9.64) by an amount equal to ${\Delta }u$, that is $u^{*}=u+{\Delta }u$

$$\begin{aligned} \begin{array}{c} \dot{x}_d=Ax_d+Bu^{*}+d_2 \end{array} \end{aligned}$$

(9.65)

The dynamics of the controlled system described in Eq. (9.64) can be also written as

$$\begin{aligned} \begin{array}{c} \dot{x}=Ax+Bu+Bu^{*}-Bu^{*}+d_1 \end{array} \end{aligned}$$

(9.66)

and by denoting $d_3=-Bu^{*}+d_1$ as an aggregate disturbance term one obtains

$$\begin{aligned} \begin{array}{c} \dot{x}=Ax+Bu+Bu^{*}+d_3 \end{array} \end{aligned}$$

(9.67)

By subtracting Eq. (9.65) from Eq. (9.67) one has

$$\begin{aligned} \begin{array}{c} \dot{x}-\dot{x}_d=A(x-x_d)+Bu+d_3-d_2 \end{array} \end{aligned}$$

(9.68)

By denoting the tracking error as $e=x-x_d$ and the aggregate disturbance term as $\tilde{d}=d_3-d_2$, the tracking error dynamics becomes

$$\begin{aligned} \begin{array}{c} \dot{e}=Ae+Bu+\tilde{d} \end{array} \end{aligned}$$

(9.69)

The above linearized form of the UAV’s model can be efficiently controlled after applying an H-infinity feedback control scheme.

9.3.4.2 The Nonlinear H-Infinity Control

The initial nonlinear model of the unmanned aerial vehicle is in the form

$$\begin{aligned} \begin{array}{c} \dot{x}=f(x, u) \ \ x~{\in }~R^n, \ u~{\in }~R^m \end{array} \end{aligned}$$

(9.70)

Linearization of the system (multi-DOF UAV) is performed at each iteration of the control algorithm round its present operating point ${(x^{*},u^{*})}=(x(t), u(t-T_s))$. The linearized equivalent of the system is described by

$$\begin{aligned} \begin{array}{c} \dot{x}=Ax+Bu+L\tilde{d} \ \ x~{\in }~R^n, \ u~{\in }~R^m, \ \tilde{d}~{\in }~R^q \end{array} \end{aligned}$$

(9.71)

where matrices A and B are obtained from the computation of the Jacobians of the UAV’s state-space description and vector $\tilde{d}$ denotes disturbance terms due to linearization errors. The problem of disturbance rejection for the linearized model that is described by

$$\begin{aligned} \begin{array}{c} \dot{x}=Ax+Bu+L\tilde{d} \\ y=Cx \end{array} \end{aligned}$$

(9.72)

where $x~{\in }~R^n$, $u~{\in }~R^m$, $\tilde{d}~{\in }~R^q$ and $y~{\in }~R^p$, cannot be handled efficiently if the classical LQR control scheme is applied. This is because of the existence of the perturbation term $\tilde{d}$. The disturbance term $\tilde{d}$ apart from modeling (parametric) uncertainty and external perturbations can also represent noise terms of any distribution.

As analyzed in previous applications of the $H_{\infty }$ control approach, a feedback control scheme is designed for trajectory tracking by the system’s state vector and simultaneous disturbance rejection, considering that the disturbance affects the system in the worst possible manner. The disturbances’ effects are incorporated in the following quadratic cost function:

$$\begin{aligned} \begin{array}{c} J(t)={1 \over 2}{\int _0^T}[{y^T}(t)y(t)+r{u^T}(t)u(t)-{\rho ^2}{\tilde{d}^T}(t)\tilde{d}(t)]dt, \ \ r,{\rho }>0 \end{array} \end{aligned}$$

(9.73)

The significance of the negative sign in the cost function’s term that is associated with the perturbation variable $\tilde{d}(t)$ is that the disturbance tries to maximize the cost function J(t) while the control signal u(t) tries to minimize it. The physical meaning of the relation given above is that the control signal and the disturbances compete to each other within a min-max differential game. This problem of min-max optimization can be written as

$$\begin{aligned} \begin{array}{c} {min_{u}}{max_{\tilde{d}}}J(u,\tilde{d}) \end{array} \end{aligned}$$

(9.74)

In the previous cases of applications of the H-infinity control it has been explained that the objective of the optimization procedure is to compute a control signal u(t) which can compensate for the worst possible disturbance, that is externally imposed to the system. However, the solution to the min-max optimization problem is directly related to the value of the parameter $\rho$. This means that there is an upper bound in the disturbances magnitude that can be annihilated by the control signal.

9.3.4.3 Computation of the Feedback Control Gains

For the linearized system given by Eq. (9.72) the cost function of Eq. (9.73) is defined, where the coefficient r determines the penalization of the control input and the weight coefficient $\rho$ determines the reward of the disturbances’ effects.

Once more it is assumed that (i) The energy that is transferred from the disturbances signal $\tilde{d}(t)$ is bounded, that is ${\int _0^{\infty }}{\tilde{d}^T(t)}\tilde{d}(t){dt}<\infty$, (ii) the matrices [A, B] and [A, L] are stabilizable, (iii) the matrix [A, C] is detectable. Then, the optimal feedback control law is given by

$$\begin{aligned} \begin{array}{c} u(t)=-Kx(t) \end{array} \end{aligned}$$

(9.75)

with

$$\begin{aligned} \begin{array}{c} K={1 \over r}{B^T}P \end{array} \end{aligned}$$

(9.76)

where P is a positive semi-definite symmetric matrix which is obtained from the solution of the Riccati equation

$$\begin{aligned} \begin{array}{c} {A^T}P+PA+Q-P({1 \over r}B{B^T}-{1 \over {2\rho ^2}}L{L^T})P=0 \end{array} \end{aligned}$$

(9.77)

where Q is also a positive definite symmetric matrix. The worst case disturbance is given by

$$\begin{aligned} \begin{array}{c} \tilde{d}(t)={1 \over \rho ^2}{L^T}Px(t) \end{array} \end{aligned}$$

(9.78)

The diagram of the considered control loop is depicted in Fig. 9.10.

Fig. 9.10

Diagram of the control scheme for the multi-DOF UAV

9.3.5 Lyapunov Stability Analysis

Through Lyapunov stability analysis it will be shown that the proposed nonlinear control scheme assures $H_{\infty }$ tracking performance for the UAV, and that in case of bounded disturbance terms asymptotic convergence to the reference setpoints is achieved. The tracking error dynamics for the multi-DOF unmanned aerial vehicle is written in the form

$$\begin{aligned} \begin{array}{c} \dot{e}=Ae+Bu+L\tilde{d} \end{array} \end{aligned}$$

(9.79)

where in the robot’s case $L=I~{\in }~R^{12}$ with I being the identity matrix. Variable $\tilde{d}$ denotes model uncertainties and external disturbances of the UAV’s model. The following Lyapunov function is considered

$$\begin{aligned} \begin{array}{c} V={1 \over 2}{e^T}Pe \end{array} \end{aligned}$$

(9.80)

where $e=x-x_d$ is the tracking error. By differentiating with respect to time one obtains

$$\begin{aligned} \begin{array}{c} \dot{V}={1 \over 2}{\dot{e}^T}Pe+{1 \over 2}eP\dot{e}{\Rightarrow }\\ \dot{V}={1 \over 2}{[Ae+Bu+L\tilde{d}]^T}Pe+{1 \over 2}{e^T}P[Ae+Bu+L\tilde{d}]{\Rightarrow } \\ \end{array} \end{aligned}$$

(9.81)

$$\begin{aligned} \begin{array}{c} \dot{V}={1 \over 2}[{e^T}{A^T}+{u^T}{B^T}+{\tilde{d}^T}{L^T}]Pe+\\ +{1 \over 2}{e^T}P[Ae+Bu+L\tilde{d}]{\Rightarrow } \\ \end{array} \end{aligned}$$

(9.82)

$$\begin{aligned} \begin{array}{c} \dot{V}={1 \over 2}{e^T}{A^T}Pe+{1 \over 2}{u^T}{B^T}Pe+{1 \over 2}{\tilde{d}^T}{L^T}Pe+ \\ {1 \over 2}{e^T}PAe+{1 \over 2}{e^T}PBu+{1 \over 2}{e^T}PL\tilde{d} \end{array} \end{aligned}$$

(9.83)

The previous equation is rewritten as

$$\begin{aligned} \begin{array}{c} \dot{V}={1 \over 2}{e^T}({A^T}P+PA)e+({1 \over 2}{u^T}{B^T}Pe+{1 \over 2}{e^T}PBu)+\\ +({1 \over 2}{\tilde{d}^T}{L^T}Pe+{1 \over 2}{e^T}PL\tilde{d}) \end{array} \end{aligned}$$

(9.84)

Assumption: For given positive definite matrix Q and coefficients r and $\rho$ there exists a positive definite matrix P, which is the solution of the following matrix equation

$$\begin{aligned} \begin{array}{c} {A^T}P+PA=-Q+P({2 \over r}B{B^T}-{1 \over \rho ^2}L{L^T})P \end{array} \end{aligned}$$

(9.85)

Moreover, the following feedback control law is applied to the system

$$\begin{aligned} \begin{array}{c} u=-{1 \over {r}}{B^T}Pe \end{array} \end{aligned}$$

(9.86)

By substituting Eqs. (9.85) and (9.86) one obtains

$$\begin{aligned} \begin{array}{c} \dot{V}={1 \over 2}{e^T}[-Q+P({2 \over r}B{B^T}-{1 \over \rho ^2}L{L^T})P]e+\\ +{e^T}PB(-{1 \over {r}}{B^T}Pe)+{e^T}PL\tilde{d}{\Rightarrow } \end{array} \end{aligned}$$

(9.87)

$$\begin{aligned} \begin{array}{c} \dot{V}=-{1 \over 2}{e^T}Qe+{1 \over {r}}PB{B^T}Pe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe\\ -{1 \over {r}}({e^T}PB{B^T}Pe)+{e^T}PL\tilde{d} \end{array} \end{aligned}$$

(9.88)

which after intermediate operations gives

$$\begin{aligned} \begin{array}{c} \dot{V}=-{1 \over 2}{e^T}Qe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe+{e^T}PL\tilde{d} \end{array} \end{aligned}$$

(9.89)

or, equivalently

$$\begin{aligned} \begin{array}{c} \dot{V}=-{1 \over 2}{e^T}Qe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe+\\ +{1 \over 2}{e^T}PL\tilde{d}+{1 \over 2}{\tilde{d}^T}{L^T}Pe \end{array} \end{aligned}$$

(9.90)

Lemma: The following inequality holds

$$\begin{aligned} \begin{array}{c} {1 \over 2}{e^T}PL\tilde{d}+{1 \over 2}\tilde{d}{L^T}Pe-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe{\le }{1 \over 2}{\rho ^2}{\tilde{d}^T}\tilde{d} \end{array} \end{aligned}$$

(9.91)

Proof: The binomial $({\rho }{\alpha }-{1 \over \rho }b)^2$ is considered. Expanding the left part of the above inequality one gets

$$\begin{aligned} \begin{array}{c} {\rho ^2}{a^2}+{1 \over {\rho ^2}}{b^2}-2ab \ge 0 \Rightarrow {1 \over 2}{\rho ^2}{a^2}+{1 \over {2\rho ^2}}{b^2}-ab \ge 0 \Rightarrow \\ ab-{1 \over {2\rho ^2}}{b^2} \le {1 \over 2}{\rho ^2}{a^2} \Rightarrow {1 \over 2}ab+{1 \over 2}ab-{1 \over {2\rho ^2}}{b^2} \le {1 \over 2}{\rho ^2}{a^2} \end{array} \end{aligned}$$

(9.92)

The following substitutions are carried out: $a=\tilde{d}$ and $b={e^T}{P}L$ and the previous relation becomes

$$\begin{aligned} \begin{array}{c} {1 \over 2}{\tilde{d}^T}{L^T}Pe+{1 \over 2}{e^T}PL\tilde{d}-{1 \over {2\rho ^2}}{e^T}PL{L^T}Pe{\le }{1 \over 2}{\rho ^2}\tilde{d}^T\tilde{d} \end{array} \end{aligned}$$

(9.93)

Equation (9.93) is substituted in Eq. (9.90) and the inequality is enforced, thus giving

$$\begin{aligned} \begin{array}{c} \dot{V}{\le }-{1 \over 2}{e^T}Qe+{1 \over 2}{\rho ^2}{\tilde{d}^T}\tilde{d} \end{array} \end{aligned}$$

(9.94)

Equation (9.94) shows that the $H_{\infty }$ tracking performance criterion is satisfied. The integration of $\dot{V}$ from 0 to T gives

$$\begin{aligned} \begin{array}{c} {\int _0^T}\dot{V}(t)dt{\le }-{1 \over 2}{\int _0^T}{||e||_Q^2}{dt}+{1 \over 2}{\rho ^2}{\int _0^T}{||\tilde{d}||^2}{dt}{\Rightarrow }\\ 2V(T)+{\int _0^T}{||e||_Q^2}{dt}{\le }2V(0)+{\rho ^2}{\int _0^T}{||\tilde{d}||^2}dt \end{array} \end{aligned}$$

(9.95)

Moreover, if there exists a positive constant $M_d>0$ such that

$$\begin{aligned} \begin{array}{c} \int _0^{\infty }{||\tilde{d}||^2}dt \le M_d \end{array} \end{aligned}$$

(9.96)

then one gets

$$\begin{aligned} \begin{array}{c} {\int _0^{\infty }}{||e||_Q^2}dt \le 2V(0)+{\rho ^2}{M_d} \end{array} \end{aligned}$$

(9.97)

Thus, the integral ${\int _0^{\infty }}{||e||_Q^2}dt$ is bounded. Moreover, V(T) is bounded and from the definition of the Lyapunov function V in Eq.  (9.80) it becomes clear that e(t) will be also bounded since $e(t) \ \in \ \varOmega _e=\{e|{e^T}Pe{\le }2V(0)+{\rho ^2}{M_d}\}$. According to the above and with the use of Barbalat’s Lemma one obtains $lim_{t \rightarrow \infty }{e(t)}=0$.

9.3.6 Robust State Estimation with the Use of the H-Infinity Kalman Filter

Another problem that has to be dealt with in the design of a state feedback controller for the UAV (autonomous quadrotor) is that in several operating conditions the complete state vector might not be measurable. Actually, attempting to measure the complete state vector with the use of suitable sensors is not only costly but is also error-prone because, particularly in the harsh operating environment of the UAVs. Thus the control loop has to be implemented with the use of information provided by a small number of sensors and by processing only a small number of state variables. To reconstruct the missing information about the state vector of the quadrotor it is proposed to use a filtering scheme and based on it to apply state estimation-based control [33, 169, 431, 463, 511].

The recursion of the $H_{\infty }$ Kalman Filter, for the model of the six-DOF UAV, can be formulated in terms of a measurement update and a time update part

Measurement update:

$$\begin{aligned} \begin{array}{l} D(k)=[I-{\theta }W(k)P^{-}(k)+{C^T}(k)R(k)^{-1}C(k)P^{-}(k)]^{-1}\\ K(k)=P^{-}(k)D(k){C^T}(k)R(k)^{-1}\\ \hat{x}(k)=\hat{x}^{-}(k)+K(k)[y(k)-C\hat{x}^{-}(k)] \end{array} \end{aligned}$$

(9.98)

Fig. 9.11

Control of the quadrotor when tracking flight path 1: a Three-dimensional plot of tracking of flight path 1 by the quadropter, b Cartesian coordinates of the UAV and convergence to the reference setpoints

Time update:

$$\begin{aligned} \begin{array}{l} \hat{x}^{-}(k+1)=A(k)x(k)+B(k)u(k) \\ P^{-}(k+1)=A(k)P^{-}(k)D(k)A^T(k)+Q(k) \end{array} \end{aligned}$$

(9.99)

where it is assumed that parameter $\theta$ is sufficiently small to assure that the term ${P^{-}(k)}^{-1}-{\theta }W(k)+C^T(k)R(k)^{-1}C(k)$ will be positive definite. When $\theta =0$ the $H_{\infty }$ Kalman Filter becomes equivalent to the standard Kalman Filter. It is noted that apart from the process noise covariance matrix Q(k) and the measurement noise covariance matrix R(k) the $H_\infty$ Kalman filter requires tuning of the weight matrices L and S, as well as of parameter $\theta$.

Fig. 9.12

Control of the quadrotor when tracking flight path 1: a Convergence of state variables $x_1$ to $x_4$ to the reference setpoints, b Convergence of state variables $x_5$ to $x_8$ to the reference setpoints

Fig. 9.13

Control of the quadrotor when tracking flight path 1: a Convergence of state variables $x_9$ to $x_{12}$ to the reference setpoints, b Control inputs $u_1$ to $u_4$ exerted on the UAV

Fig. 9.14

Control of the quadrotor when tracking flight path 2: a Three-dimensional plot of tracking of flight path 1 by the quadropter, b Cartesian coordinates of the UAV and convergence to the reference setpoints

In the case of UAVs (e.g. autonomous quadropters), the H-infininty Kalman Filter can be used within a state estimation-based control scheme. Actually, one can measure only a part of the state vector of the UAV, such as state variables $x_1=x$, $x_3=y$, $x_5=z$, $x_7=\phi$, $x_9=\theta$, $x_{11}=\psi$ and estimate through filtering the rest of the state vector elements that is $x_2=\dot{x}$, $x_4=\dot{y}$, $x_5=\dot{z}$, $x_7=\dot{\phi }$, $x_9=\dot{\theta }$, $x_{11}=\dot{\psi }$. Moreover, the proposed Kalman filtering method can be used for sensor fusion purposes.

9.3.7 Simulation Tests

The tracking performance of the considered nonlinear H-infinity control scheme was tested in the case of several reference flight paths. The first 3D reference trajectory is shown in Fig. 9.11, while the convergence of the UAV’s setpoints to their setpoints are shown in Figs. 9.12a, b and 9.13a. The control inputs exerted on the UAV by its actuators are shown in Fig. 9.13b.

Fig. 9.15

Control of the quadrotor when tracking flight path 2: a Convergence of state variables $x_1$ to $x_4$ to the reference setpoints, b Convergence of state variables $x_5$ to $x_8$ to the reference setpoints

Fig. 9.16

Control of the quadrotor when tracking flight path 2: a Convergence of state variables $x_9$ to $x_{12}$ to the reference setpoints, b Control inputs $u_1$ to $u_4$ exerted on the UAV

Fig. 9.17

Control of the quadrotor when tracking flight path 3: a Three-dimensional plot of tracking of flight path 1 by the quadropter, b Cartesian coordinates of the UAV and convergence to the reference setpoints

Fig. 9.18

Control of the quadrotor when tracking flight path 3: a Convergence of state variables $x_1$ to $x_4$ to the reference setpoints, b Convergence of state variables $x_5$ to $x_8$ to the reference setpoints

Fig. 9.19

Control of the quadrotor when tracking flight path 3: a Convergence of state variables $x_9$ to $x_{12}$ to the reference setpoints, b Control inputs $u_1$ to $u_4$ exerted on the UAV

The second 3D reference trajectory is shown in Fig. 9.14, while the convergence of the UAV’s setpoints to their setpoints are shown in Figs. 9.15a, b and 9.16a. The control inputs exerted on the UAV by its actuators are shown in Fig. 9.16b.

Finally, the third considered 3D reference trajectory is shown in Fig. 9.17, while the convergence of the UAV’s setpoints to their setpoints are shown in Figs. 9.18a, b and 9.19a. The control inputs exerted on the UAV by its actuators are shown in Fig. 9.19b.

It can be noticed that in all cases the nonlinear H-infinity control algorithm for the UAV achieved accurate tracking of the reference path and fast convergence to them. All state variables of the system converged fast and smoothly to the reference setpoints while their tracking error was rapidly eliminated. Moreover, the variation of the control inputs exerted on the UAV by its actuators was smooth and no abrupt changes of the control signal were observed. The above are indicative of the excellent tracking and stability properties of the nonlinear H-infinity control algorithm.