Extended Kalman Filter Based States Estimation of Unmanned Quadrotors for Altitude-Attitude Tracking Control

In this paper, state variables estimation and Fuzzy Sliding Mode Control (FSMC) are presented in order to estimate the state variables and altitudeattitude tracking control in presence of internal and external disturbances for unmanned quadrotor. The main idea of the proposed control strategy is the development of an Extended Kalman Filter (EKF) for the observation of the states. Fuzzy logic systems are used to adapt the unknown switching-gains to eliminate the chattering phenomenon induced by Sliding Mode Control (SMC). The stability of the system is guaranteed in the sense of Lyapunov. The effectiveness and robustness of the proposed controller-observer scheme that takes into account internal and external disturbances are demonstrated on computer simulation using Matlab environment.


Introduction
In recent years, Unmanned Aerial Vehicles (UAVs) have became a topic of interest in many research organizations due to their wide applications in several areas, such as enforcement of traffic rules and road networks surveillance, industrial plants and high-tension power lines, mapping three-dimensional environments.
A quadrotor is a typically under-actuated because it has six degrees of freedom and only four actual inputs, and it is a nonlinear coupled system due to the aerodynamics of its four rotors.Moreover, during the flight in low attitude, it is susceptible, which influences the flight performance or even leads to instability [1] and [2].However, the difficulty of a controller design increases due to these constraintes.Altitude and attitude tracking control are of main interests in quadrotors study.It has attracted much attention from researchers due to its potential practical applications [3].The attitude controller is an important feature since it allows the vehicle to maintain a desired orientation and hence prevents the vehicle from flipping over and crashing [4].
In the literature, several control algorithms have been proposed to altitude and attitude control of unmanned quadrotor systems such as backstepping based controller [5], fuzzy logic based controller, and sliding mode controller [6] and [7].Sliding control is well known for its effectiveness through the theoretical studies versus the parameter variations and disturbances, and has been widely applied to robotics and aircraft control design.Chattering phenomenon (high frequency of control action) is the major problem associated with SMC, which is caused by the inappropriate selection of the switching gain.In order to reduce the chattering phenomenon, various methods have been proposed in the literature [7] and [8].
In practice, the state variables of a given system are rarely available for direct measurement.The elaboration of a control law for this system often requires access to the value of one or more of its states.For this reason, it is necessary to design an auxiliary dynamic system named observer, capable to deliver state estimates from the measurements provided by physical sensors and applied inputs.In the case of nonlinear systems, there is not a general solution to the problem of observer synthesis, which prompted researchers to develop nonlinear observers such as sliding mode observer [9], High order sliding mode observer [10], Ex-tended Kalman filter [11], fuzzy logic observer [12], and artificial neural network observer [13].
EKF is an optimal recursive estimator, which can be used for estimating the state of nonlinear systems [14].In the last decade, EKF was widely used for stochastic nonlinear systems.EKF provides the suboptimal state estimator for its ability to consider the stochastic uncertainties, which is the case of quadrotor UAVs.EKF is a recursive algorithm, and it is known for its high convergence rate, which improves transient performance significantly.Compared to other nonlinear observers [15], [16], [17], [18] and [19], EKF algorithm has better dynamic behavior, resistance to uncertainties and noise, and it can work even in the presence of a standstill conditions.
In [20], the authors presented stabilizing control laws synthesis using sliding mode technique, and used the EKF to estimate the unmeasured states.The controlled model was very simple because the authors did not consider the nonlinearities, uncertainties, external perturbations, and nonholonomic constraints.
In [21], the authors presented a state estimation of the quadrotor UAV based on a high degree cubature Kalman filter, but they did not illustrate the design method of the controller and stability analysis of closed-loop system.
In [22], a Radical Basis Function Neural Networks (RBFNNs) and double-loop integral sliding mode control are presented for the position and attitude tracking of quadrotor subjected to parameter uncertainties and sustained disturbances.In [23], a novel adaptive fuzzy gain-scheduling sliding mode control is studied for attitude regulation of an unmanned quadrotors with parametric uncertainties and external disturbances.In [24], the authors explored the design of an SMC for trajectory tracking for an unmanned aerial vehicle.Note that in both of last two cited works, the influences of the nonholonomic constraints and the observation problem were not discussed.Moreover, in [25], the authors designed a fuzzy sliding mode control based on backstepping synthesis for unmanned quadrotors, in which the observation problem was not considered.
In this investigation, two major contributions are proposed: • An EKF for estimating the state space vector.
• A robust altitude-attitude Fuzzy Sliding Mode Controller (FSMC) in the presence of both internal and external disturbances.
In order to reduce chattering phenomenon, a Fuzzy Logic System (FLS) is designed to adapt the unknown switching-gains by fuzzifying the sliding surface according to the fuzzy inference rule base.Lyapunov synthesis is used to determine the stability analysis of the closed-loop system.Performances of the controllerobserver are illustrated by a simulation study that takes into account internal disturbances including the Gaussian white noise process and measurement noise, and external disturbances.
The presented paper is organized as follows.In Sec. 2. , dynamic modeling of a quadrotor is introduced.In Sec. 3. , state space representation and discretization of the quadrotor model are formulated, whereas in Sec. 4. , we recall the basic mathematical formulas of the EKF.Section 5.
is dedicated to the proposed controller-observer design and closed loop stability.In Sec. 6. , we present simulation studies that illustrate the performance of the proposed controller-observer system.Finally, conclusions are summarized in Sec. 7.

Quadrotor Dynamic Modelling
The quadrotor adopted in this investigation was introduced in [1] and [2] (see Fig. 1).Using the Newton-Euler laws of mechanics , the motion equations of the quadrotor can be written as follows: where m is the total mass of the quadrotor, d represents the disturbances applied to the quadrotor, l is the distance between the mass centre of the quadrotor and the rotation axis of propeller, and Ω = (ω 1 −ω 2 +ω 3 −ω 4 ) is the total gyroscopic torques which affect the quadrotor.K f a = diag(K f ax , K f ay , K f az ) represents the aerodynamics frictions factors.J r is the total rotational moment of inertia around the propeller axis.
Finally, u x and u y are two virtual control inputs: From Eq. ( 2), it is easy to show that :

State Space Representation
and Y = (x 1 , x 3 , x 5 , x 7 , x 9 , x 11 ) T be the state, the control input and the output vectors, respectively.
The dynamic model given in Eq. ( 1) can be written using the state space method as: where in which: Since the Kalman filter is a discrete algorithm, discretization of the model is needed.The resulting global discrete form will be given by the following discrete nonlinear representation: where ∆t is the sampling period and k ∈ Z is the discretetime points.

Extended Kalman Filter
The Kalman filter was developed by R. E. Kalman in 1960 [26].EKF is a generalization of the Kalman filter which is a stochastic observer for nonlinear dynamical systems.In this paper, we shall attempt to find the best estimate of the state vector X k of the system, which evolves according to the following discrete-time nonlinear dynamic: where f (.) represents the evolution function of the system, h(.) represents the relationship between the state vector and the measurement result Z k , whereas u k stands for the control input to the system at step k, and w k and v k are the process and measurement white Gaussian noise vectors with zero mean and with associated covariance matrices To apply EKF to the nonlinearity given in Eq. ( 7), it must be linearized by using the first order Taylor approximation around the desired reference point ( Xk , ŵk = 0, vk = 0), which gives us the following approximated linear model: where the Jacobean matrices of f and h are given as follows: .
The EKF is a recursive algorithm that is used for estimating state vector.It can be given by the following recursive equations: Prediction: Correction: Kalman filter gain matrix: where Xk+1/k+1 denotes the posteriori state prediction vector, Xk+1/k is the priori state prediction vector, P k+1/k+1 denotes the posteriori prediction error covariance matrix, and P k+1/k is the priori prediction error covariance matrix.
EKF framework is presented in Fig. 2.

Controller Design and Stability Analysis
For the development of the control laws, the following assumptions are needed: The pitch, roll, and yaw angles satisfy the following inequalities: The function f is assumed to be known and the error on its estimate is bounded, i.e. f ( X) − f (X) ≤ M , where X is an estimate have used an EKohave used an EKf X, and M < 0. Assumption 3. The input gain g is assumed to be known and bounded, i.e. 0 < g min ≤ g(X) ≤ g max .Assumption 4. The disturbances d in the discrete dynamic model are unknown but bounded, i.e.|d| ≤ D, where D ≥ 0. Assumption 5.The desired trajectory X d and its first and second time derivatives are available and assumed to be bounded.
The objective is to estimate the velocity {x 2 , x 4 , x 6 , x 8 , x 10 , x 12 } = φ, θ, ψ, ẋ, ẏ, ż , and to design a robust tracking controller so that the state vector In this section, a Fuzzy Sliding Mode Controller (FSMC) is designed; this controller combines the advantage of the SMC with FLS for the quadrotor aircraft robot.The SMC is designed to ensure the trajectory tracking and robustness against the internal and external disturbances.A FLS is designed to adapt the unknown switching-gains to reduce chattering phenomenon from fuzzifying the sliding surface according to the fuzzy inference rule base.The structure of the proposed controller-observer is shown in Fig. 3.
Motivated by practice, the measured UAV variables are linear position and yaw angle (x, y, z and ψ) and non-measurable states can be obtained by successive differentiation.However, they are contaminated by the measurement noise to a degree that the differentiation can no longer be used.For this reason, it is necessary to design an auxiliary dynamic system named observer, in this paper we have used an EKF as in Eq. ( 9), Eq. ( 10) and Eq.(11) ables and estimated velocity Ŷ of EKF are set to be inputs to the controller through a comparator.Note that the control will be impossible if the velocity cannot be measured.
If the initial condition e (0) = 0, the tracking problem X = X d can be considered as the state error vector remaining on the sliding surface s (e) = 0. To achieve this condition, a Lyapunov function candidate is defined as: A sufficient condition for the stability of the system is given in [27] as follows: This leads to the following convergence condition: where η > 0, and sgn(.)denotes the signum function, it is defined as: Note that in Eq. ( 16): |s| = s sgn (s) , by deriving Eq. ( 12): Consider the control problem of the nonlinear system given in Eq. ( 5), replacing Ẍ with the state space equations, we get: where f = (f 1 , f 2 , ..., f 6 ) T and g = (g 1 , g 2 , ..., g 6 ) T .
Consider the EKF defined by Eq. ( 9), Eq. ( 10) and Eq.(11) , by replacing X with X, we get: From Eq. ( 13) and Eq. ( 19), and to satisfy the sliding condition Eq. ( 15), u * has to be chosen so that V is negative semi definite.Therefore, we can easily construct the SMC law: where The equivalent control Eq. ( 21) is given by the following condition [27] s = 0 and ṡ = 0 ⇒ u = u eq .Based on assumptions 1-4 and considering that the estimate g( X) could be chosen according to the geometric mean g( X) = √ g max • g min , the bounds of g(X) may be expressed as β −1 < g( X)/g(X) < β, where β = g max /g min .
Under this condition, the switching-gains K should be chosen according to: where η is a strictly positive constant related to the reaching-time and τ = −f ( X) − d − λ. ė + Ẍd .
However, the use of discontinuous sgn function in the discontinuous control Eq. ( 20) will excite undesired phenomenon called chatter, which is caused by the inappropriate selection of the switching-gains matrix.In this context, high switching-gains matrix entries K in Eq. ( 20) will increase the oscillations in the control signal, and therefore an excitation of high frequency dynamics will take place, as a result, a chattering phenomenon will be created.Moreover, a decrease in switching-gains can reduce the chattering phenomenon and improve the tracking performance despite noise and external disturbances.To achieve more appropriate performance, these switching-gains must be adjusted.This adjustment is based on the distance between the system states and the sliding surfaces.i.e., when the trajectory of the system states deviate from the sliding surfaces, the switching-gains should be increased in order to reduce chattering and vice versa.This idea can be realized by combining fuzzy logic with sliding mode control to construct FSMC to facilitate the adaptive switch-gains (see Fig. 3) according to some appropriate fuzzy rules.For this reason, oneinput one-output FLS is designed in the next section.

Fuzzy Logic System
According to fuzzy logic systems [28], The fuzzy sets are defined as follows: Based on the experiences, the type of fuzzy rules is decided as "IF-THEN ".
The membership functions of input and output are chosen as illustrated in Fig. 4, in which the following linguistic variables have been used: Negative Big (NB), Negative Medium (NM), Zero (Z), Positive Medium (PM), and Positive Big (PB).
The fuzzy base rule of the adopted FLS contains five rules given in Tab. 1.
These rules govern the input-output relationship between s and K f uzzy by adopting the Mamdani-type in- Tab.1: Fuzzy rule set.
ference engine, in which the center of gravity method is used for defuzzification as in Eq. ( 23).
where N is the total number of fuzzy IF-THEN rules in the rule base, n is the number of system states.A l j and B l j denote fuzzy sets, ζ l is the centre of gravity of the membership function of K f uzzy for the l th rule.The Architecture of the FLS is shown in Fig. 5. Referring to Eq. ( 10), Eq. ( 11) and Eq. ( 13), the control law FSMC is now chosen as: where K f uzzy = diag( k1 , k2 , ..., k6 ), ki > 0 are the switching-gains matrix entries estimated by FLS.Note that the same FLS is used for all controllers (see Fig. 3).

Stability Analyses of the Proposed Controller-Observer
In order to demonstrate the stability of the closed-loop system, we adopt the following theorem: Theorem 1.Consider the nonlinear system of Eq. ( 5) associated with the chosen sliding surfaces s = 0.Under the assumptions 1-5, the estimation X given by Eq. ( 9), (10), and ( 11), the switching-gains matrix K f uzzy adjusted by the FLS Eq. ( 19) and according to Eq. ( 22), therefore condition of Eq. ( 16) is satisfied and the tracking error will converge to zero asymptotically.
Proof.Proving the theorem above requires examining the sliding condition using the Lyapunov method.
Define the following positive definite Lyapunov function candidate V = 1 2 s T s.Its time derivative is given by: Substituting Eq. ( 18) in Eq. ( 25) we get: Replacing the control u by Eq. ( 24), so that the derivative of the Lyapunov function becomes Therefore, considering Assumptions 2. and Assumptions 3., and defining K f uzzy according to Eq. ( 22), V becomes V ≤ −η |s|.
Dividing by |s| and integrating both sides over the interval 0 < t < t s (t s is the time required to reach s), gives: Considering t reach as the time required to reach s and noting that |s(t reach ) = 0|, one has t reach ≤ |s(0)| /η and, consequently, the finite time convergence to the sliding surface s.Thus, the tracking errors X − X d converge asymptotically to zero as t → +∞.
The synthesized control laws are given as follows: Altitude Control The altitude FSMC can be obtained by similar design procedures: where µ 6 , λ 6 are positive real numbers.
From the dynamic Eq. ( 1), it can be seen that the motion through the axes x and y depends on u 1 .In fact, u 1 is the total thrust vector oriented to obtain the desired linear motion, by considering u x and u y are directing of u 1 responsible for the motion through x and y axes, respectively.Using FSMC, the control motion in the direction of x and y are obtained using the same steps described above.

2) Attitude Control
Similar steps can be followed to design attitude FSMC laws for trajectory tracking control of roll, pitch, and yaw angles.The corresponding control laws are de-signed as follows, where µ i and λ i , (i = 1, 2, 3) are positive real numbers.
The simplification of all computation steps concerning the tracking errors and sliding surfaces is defined as follows, respectively.

Simulation and Discussions
In this section, the numerical simulation is conducted to demonstrate the performance of the controllerobserver developed.In this simulation, the system's nominal parameters are shown in Tab. 2. The proposed controller-observer applied to the above quadrotor is simulated on a PC using MatLab environment (version 8.6.0.267246).A total of N = 10000 measurement data are simulated on a time interval from 0 to 10 seconds with step size ∆t = 0.001 s.Note that the full program is coded in Matlab M-Files.
K f a = diag(5.5670;5.5670; 6.3540)    13 show the simulation results tained by the proposed method.Figure 6 and Fig. 7 show the output positions tracking and the positions tracking errors, respectively, where we observe that, the performances and robustness of the fuzzy sliding mode controller under the occurrence of internal and external disturbances are very acceptable.Figure 8 and Fig. 10 represent the velocities tracking, the velocities estimation errors, and the velocities tracking errors, respectively.It is clear that the estimated states converge to the desired ones (see Fig. 8), which shows a satisfactory estimation during the flight see Fig. 9.As it is clear from these figures, although we did not have measurements on velocities, EKF was able to es-  timate these values significantly.Figure 10 shows the velocities tracking errors, which all tend to zero after a finite time.The corresponding control inputs are depicted in Fig. 11, where we can clearly see that the chattering almost disappeared.It is noted that the discontinuities amplitudes are reduced.Compared  to the conventional SMC [8], we see clearly that the proposed control approach effectively reduces chattering phenomenon.The estimated fuzzy switching-gain curves are depicted in Fig. 12.We have also illustrated

Conclusion
In this paper, we presented an EKF based states estimation, and altitude-attitude tracking control for unmanned quadrotor despite the presence of internal and external disturbances.We assumed that not all states are measured; therefore an EKF system to estimate the hidden states in order to avoid hardware sensors was introduced.Sliding mode control was applied, and it was enhanced by a fuzzy system to adapt the unknown switching-gains in order to eliminate the chattering phenomenon.Based on the simulation results, we conclude that the proposed controller-observer performs well.This reflect the robustness and performances of the mixed controller-observer, which was also confirmed by the tracking an estimation errors convergence.The stability of the proposed approach was guaranteed by Lyapunov stability criterion.Simulation results confirmed the ability of proposed controllerobserver to ensure a good estimation and yield superior control performances for nonlinear system control against internal and external disturbance simultaneously.
even in presence of large external disturbances in the dynamic model.The controller is designed in three steps: Altitude control, position control (x and y motions), and attitude control (roll, pitch and yaw) as shown in Fig.3.

Figure 6 ,Fig. 7 ,
Figure 6, Fig. 7, Fig. 8, Fig. 9, Fig. 10, Fig. 11, Fig. 12 and Fig.13show the simulation results tained by the proposed method.Figure6and Fig.7show the output positions tracking and the positions tracking errors, respectively, where we observe that, the performances and robustness of the fuzzy sliding mode controller under the occurrence of internal and external disturbances are very acceptable.Figure8and Fig.10represent the velocities tracking, the velocities estimation errors, and the velocities tracking errors, respectively.It is clear that the estimated states converge to the desired ones (see Fig.8), which shows a satisfactory estimation during the flight see Fig.9.As it is clear from these figures, although we did not have measurements on velocities, EKF was able to es-