Filtered Observer-Based IDA-PBC Control for Trajectory Tracking of a Quadrotor

In this paper, a new filtered observer-based IDA-PBC (Interconnection and Damping Assignment-Passivity Based Control) strategy is developed for trajectory tracking of a quadrotor in the presence of disturbances and model uncertainties. The proposed algorithm allows the control of the quadrotor in all its states. It can deal with the noisy output measurements and uncertainties in the translational and rotational dynamics, as unmodeled dynamics inherent to real systems or unknown external signals (exogenous signals). The designed filtered observer estimates the state from noisy output measurements, and it depends only on two design parameters. Numerical simulation tests are carried out to highlight the overall controller approach in a realistic scenario.


I. INTRODUCTION
Quadrotors are widely used in everyday life, they have been used in many applications, e.g., surveillance, photography, transport of packages, agriculture, rescue, etc, [1], [2]. However, the performance of these vehicles can be degraded by the presence of disturbances, such as measurement noise and uncertainties. For the control of quadrotors, it is necessary to have specific characteristics of smoothness in both control signals and sensors, for this has been used innumerable techniques using filters [3], [4].
The use and research of observers for state estimation, perturbations, and the calculation of output controllers in drones is still very active, e.g., [5]- [8]. Given the complexity and its enormous applications, this research is still open [9]- [12]. To the best of our knowledge, there are few works oriented to the use of the novel filtered observers to applications in drones or quadrotors as [13].
Thus, the design problem of controllers and observers for quadrotors has been widely researched in the literature and is a novel subject in the current research. Within the framework The associate editor coordinating the review of this manuscript and approving it for publication was Wonhee Kim . of this theme, a continuous high-gain observer-based PD (Proportional Derivative) control for a quadrotor vehicle is synthesized in [14], where the high-gain observer estimates the state vector from the position coordinates and only one angle (yaw), the interesting of this paper is that the continuous-time estimation (the roll and pitch angles and all velocities) is achievable by using sampled measurements and a PD control. Other recent work [15] includes the problem of fault estimation for a quadrotor using a robust H ∞ observer, where the supposed faults are external disturbances, parameter uncertainties, and nonlinear terms. In [16] a cascade active disturbance rejection control and a backstepping sliding-mode control are applied to achieve the desired trajectory tracking performance of a quadrotor in the presence of external disturbances and model uncertainties. [17] presents an integral sliding mode and backstepping sliding mode controllers in a double loop control structure, which ensure the trajectory tracking capability for the desired position of a quadrotor. Also, [18] developed an observer to estimate the external wrench forces applied on a UAV (Unmanned Aerial Vehicle). The lagrangian representation of the UAV is used, which permits to shape the interactive behavior of the quadrotor using an IDA-PBC (Interconnection and Damping Assignment Passivity Based Control).
The IDA-PBC strategy has been successfully applied to mechatronic systems, for example: [19] presents a dynamic feedback control by the IDA-PBC algorithm, to stabilize a class of weakly-coupled electromechanical systems as the electrostatic MEMS devices and the magnetic levitation. [20] designs two adaptive control techniques to handle uncertainties caused by parametric and modeling errors in the Acrobot and non-prehensile planar rolling robotic (disk-on-disk) systems. The techniques use the PCH (Port-controlled Hamiltonian) modelling framework and the IDA-PBC algorithm. [21] introduces an alternative construction that yields a continuous IDA-PBC law.
Other important works are: [22], [23]. [22] presents a linear matrix inequality-based second-order sliding set control for the uncertain systems with time-varying uncertainties, nonlinearities and external disturbances. [23] develops a passivity cascade technique-based control for a nonlinear inverted pendulum system with uncertainties and exogenous disturbances.
Other approaches have addressed the observer-based control design. For example, in [24] the design problem of the observer-based adaptive sliding mode control is studied and applied to a fixed-wing UAV. In [25] an observer-based backstepping control for a quadrotor in the presence of disturbances and parametric uncertainties is proposed. The disturbance observer is used to compensate the unknown disturbance, however, it only compensates small measurement noises. [26] develops an observed-based control strategy for position-yaw tracking of quadrotors. A disturbance observer-based nonlinear controller for the trajectory tracking of a quadrotor in the presence of noisy output measurements is introduced in [27], where the translational positions have a good performance, however, the rotational positions have highly significant variations.
It is noteworthy that several relatively recent works have been focused on the improvement of the high-gain observer performances, especially in the presence of measurement noises, by proposing versions with a time-varying design parameter or by slightly changing the structure of the gain in the observer [13], [28], [29]. This new work falls within the framework of this last approach.
A very common practical situation in the real-time implementation of any observer-based control is that output measurements can be strongly disturbed by noisy measurements, resulting in high estimation errors, or worse, unstable estimation behavior. In this situation, the main problem is to prove the exponential error convergence to a neighborhood of the origin for any initial conditions of a proposed observer that provides a free noise estimation of the state vector for a quadrotor. In this sense, the main focus of this work consists in the synthesis of a novel filtered observer-based IDA-PBC control for trajectory tracking of a quadrotor, which by the observer will be capable of dealing with the noisy output measurements and uncertainties in the translational and rotational dynamics, as unmodeled dynamics inherent to real systems or unknown external signals (exogenous signals).
There is currently little work on control algorithms applied to quadrotors that can handle and thus perform well in the presence of relatively large noise signals in the output measurements and uncertainties, in fact, most of the few current designs perform well only for small values of noise signals that are present in the output measurements, e.g. [14], [25]. Furthermore, current approaches do not consider all the system dynamics to propose estimates, e.g. [24]. Moreover, most of the works consider only the presence of noise in the rotation dynamics, e.g. [27].
Unlike many works published in the literature, the proposed observer system only needs two tuning parameters, one to adjust the state estimate and the other to filter the measurement noise. Traditional observer systems use several tuning parameters and most of them perform a linearization, e.g. [9], [10]. Also, some works consider more than 4 (out of 12) elements of the state vector as measured output, e.g. [26].
Thus, motivated by the aforementioned works, in this paper, we offer an alternative approach for trajectory tracking of a quadrotor in the presence of measurement noise and uncertainty in the states by using an observer-based IDA-PBC strategy. In order to design a filtered observer that estimates the state from noisy output measurements, one represents the quadrotor dynamic model as a non-uniformly observable nonlinear system with coupled blocks, based on the proposed observer from [14], for nonlinear systems with blockstate-affine cascade structure, where a sufficient persistent excitation condition is provided. In the case when the noise signal is of high intensity or is strongly variable, the existing observer-based control design results fail to provide a good behavior. However, this paper introduces an algorithm that works for relatively large noise signals, which are present in the output measurements, and it also deals with uncertainties.
The primary aim of unmanned aerial vehicles is to follow a trajectory within the desired operating and safety limits. However, in many cases this is not possible due to the presence of measurement noise and uncertainty. It is evident that the quadrotor needs a robust controller to compensate for disturbances. Faced with this problem, the novelty of this work is to design a robust observer-based IDA-PBC control with respect to uncertainty in states and external signals that disturb the system, such as measurement noise, for trajectory tracking of a quadrotor. The contribution is then, the observer design that considers the nonlinearities of the model and that is able to estimate with a good performance, despite the presence of relatively large measurement noise and uncertainty in the state, this causes the controller performance to be adequate, i.e. even if the controller is not robust to measurement noise and uncertainty in the states, the observer will feedback to the controller signals free of measurement noise and uncertainty. In the case of many physical systems, the above is very important, for example, in the quadrotor the noise in the tracker positions has a significant impact on the accuracy of the position measurement. The paper is organized as follows: Section II presents the quadrotor dynamic model. In Section 3, the IDA-PBC strategy is developed. The filtered observer design is designed in Section IV. Before addressing the observer design for a quadrotor, some fundamental definitions are given. The performances of the proposed observer-based control are shown in Section V. Finally, conclusion is given in Section VI.

II. QUADROTOR DYNAMIC MODEL
In this section, the quadcopter dynamic model is introduced. Figure 1 shows the free-body diagram of a quadrotor, which has six degrees of freedom and only four control inputs.
As reported in Figure 1, O = e x , e y , e z is the inertial coordinate frame fixed to the ground and B = {e 1 , e 2 , e 3 } is the body fixed coordinate frame. The vector q = ν T ∈ R 6 , where ν = x y z T denotes the translational position of the vehicle. The rotational position of the quadcopter is represented by the Euler angles: roll, pitch and yaw, i,e,, = φ θ ψ T , the distance between the motors and the gravity center is represented by d. Finally, f 1 , f 2 , f 3 and f 4 are the thrust forces provided by each rotor.
The control input is defined as u = f τ T ∈ R 4 , where f is the total thrust magnitude, τ = τ ψ τ θ τ φ T is the input torques vector. Particularly for a quadcopter, the relationships between the input torques and forces are defined as where k i ω 2 i defines the thrust of the propeller of motor i w.r.t. angular velocity ω i , on each rotor. R is the rotational matrix from the body frame to the inertial one, where the shorthand notations for trigonometric functions s θ = sin(θ) and c θ = cos(θ) are used, this also applies for all angles, it yields: The mathematical model of the quadcopter for the translational and rotational motions is represented as follows [14]: where the mass of the quadcopter is introduced by m, the gravity acceleration is represented by g, the constants a 1 , a 2 , a 3 , b 1 , b 2 and b 3 are obtained as: where I x , I y and I z represent the moments of inertia of the vehicle with respect to frame B.
According to q = [ν ] T , the mathematical model (1) can be rewritten as follows:

III. IDA-PBC STRATEGY FOR A QUADROTOR
We can represent (2) as: where the inertia matrix M is constant and independent of q, C(q) represents the Coriolis matrix, G is the gravitational vector and B(q) takes the form: The total energy is where the total energy H (q, p) is a Hamiltonian function, q ∈ R 6 represents the generalized position and p ∈ R 6 introduces the generalized momentum. Then, the dynamic system is defined as where I n ∈ R 6×6 is the identity matrix, ∇ q H = ∂H /∂q and The desired Hamiltonian function can be written as: where M d = M T d > 0 introduces the closed loop inertia matrix and V d expresses the desired potential energy. It is necessary that the energy V d have an isolated minimum at q * , i.e., q * = arg min V d (q).
The PBC control signal is the sum of energy shaping and injects the damping: The desired port controlled Hamiltonian dynamics is taken as [30]: where The damping injection term is given by To compute u es we substitute (6) and (8) in (4) and make it equal to (7), then One can observe from (9) that the first row is clearly satisfied. In the second set of equations the PDEs (Partial Differential Equations) yield to the energy shaping expression given by The PDEs (9) can be separated into the equations where B(q) ⊥ is the full rank left annihilator of B(q).
To obtain the energy-shaping, we consider J 2 = 0 and M d a constant matrix of the form: the potential energy PDE (11) takes the form: which is solved and thus the desired potential energy is computed by where (q) is an arbitrary differentiable function. For this, the necessary condition ∇ q V d (q * ) = 0 and the sufficient condition ∇ 2 q V d (q * ) > 0 will hold if the Hessian of (q) at q * is positive. We choose (q) to be a quadratic function which leads to where (x d , y d , z d ) denotes the equilibrium configuration and the k px , k py , k pz terms are proportional gains and are used as tuning parameters. In order to determine the final control law, we first compute the energy-shaping expression u es from (10) and second, we obtain the damping injection expression u di from (8).

IV. DESIGN OF THE FILTERED OBSERVER
The excessive cost of the motion capture systems have motivated researchers to propose estimation and control approaches, which are able to handle this condition. Thus, a simple and cost-effective way to obtain the position ν of the quadrotor is to use a Global Positioning System (GPS), in addition, the yaw angle ψ can be obtained from a digital compass. Another obstacle of the robust control design for the quadrotor is the presence of the adverse effects as disturbance, uncertainty and noisy measurements. The output measurements are usually transmitted with measurement noise, there is therefore a peaking phenomenon of the state estimates affecting the control performance. Then, in order to propose an observer with filtering capabilities for the system (2), firstly, one defines the continuous-time measured output of the quadrotor system as: With the aim of providing a filtered observer based IDA-PBC control for the system (2) and (3), one should consider a change of coordinates : K ⊂ R 12 → M ⊂ R 12 , these set are compact and where Z = (q), in such a way that the quadrotor system (2) in their coordinates can be rendered a canonical form. Thus, the new vector of coordinates is given: . Now, the new system of coordinates is defined as follows: Consider the quadrotor positions y 1 = x and y 2 = y be the first two measured outputs, therefore, z j for j = 1, 2 and it yields: Thus, the first two subsystems of the coordinates can be written as: where C j = 1 0 0 0 and: The same procedure is also possible for the other two states, therefore the quadrotor position y 3 = z and the angular position y 4 = ψ, thus: where  (17), the purpose of this work is to propose an observer for a quadrotor, which is subject to unknown uncertainties and noise signals, i.e., these signals are present in each subsystem. This leads to the following class of block state affine nonlinear uncertain systems: p k λ k = n; p k ≥ 1 and λ k ≥ 2. u(t) ∈ R m defines the input vector. Finally, y = (y 1 , . . . , y q ) T ∈ R p denotes the output vector, with y k ∈ R p k for k = 1, . . . , q and q k=1 p k = p. The unknown function is ε = (ε 1 , . . . , ε q ) T ∈ R n and the noise signal is w = (w 1 , . . . , w q ) T ∈ R p . The matrix of the functions A and the matrix C are expressed as follows: The matrix B is defined as follows: The nonlinear function ϕ(u, x) is given by the expression: x and assumes a structural dependence on the state variables.
Additionally, in order to provide a filtered observer, the following classical assumptions are considered (see [13]) A1 The state vector and the control input are bounded, i.e., x ∈ X and u ∈ U .

A2
The function ϕ (x, u) is Lipschitz w.r.t. x uniformly w.r.t. u, with a Lipschitz constant L ϕ .

A3
The upper bound of the function A k (u) is denoted by A M = sup t≥0 A k (u) .
Thus, the following sequence of scalar numbers for k = 1, . . . , q and i = 1, . . . , λ k is denoted as: where σ k 1 = −(λ k − 1)δ k + (λ 1 − 1)δ 1 + η(1 − 1 2 k−1 ), where the term η is chosen arbitrarily within the interval η ∈ (0, 1]. Now, similarly to the k 's, one defines for k = 1, . . . , q the diagonal matrices k 's as: k ( ) = −σ k 1 k ( ). Thus, considering the forms of k ( ), k ( ), the matrices A k and C k , one obtains the next equalities: The main drawback for synthesising an observer for system (18) is that its observability depends on the input. Therefore, this should be ensured by a condition of the persistent excitation. In this sense, one needs to introduce a state transition matrix for system (18), as follows: where ξ k ∈ R n and u denotes the input. The matrix k u (t, s) is defined as follows Then, the following condition should be satisfied

A5
The input u satisfies the following persistent excitation condition: Our main result of the work is addressed to the design of an observer providing an estimation of the state in presence of the perturbed output, as noisy output measurements, also, the nonlinear system considering some uncertainties. The candidate observer for system (18) is defined as follows: for k = 1, . . . , k, wherex k = (x k 1 ,x k 2 , . . . ,x k λ k ) T ∈ R n k denotes the state estimate and the tuning parameters > 0 and µ > 0. The matrices A T k , I k (identity matrix) and C η k with dimensions n k × n k , n k × n k and 1 × n k and, respectively, are given: The observer gain is Following the above, the next main theorem can be established: Theorem 1: Consider the uncertain nonlinear system with noisy measurement outputs (18), satisfying assumptions A1-A5. If there exists two tuning parameters * and µ * for sufficiently values of and µ, such that > * and µ > µ * , system (25)-(27) provides a free noise estimation of the state vector for system (18) with an exponential error convergence to a neighborhood of the origin for any initial conditions (x k (0)) ∈ X and η(0) = 0, thus, the observation errorx k −x k closes to zero, depending onε that is the ultimate limit of uncertainties and the noise signalw.
Proof of Theorem 1: Now, we prove the convergence to a neighborhood of the origin of the i-th component of the observation errorẽ k ∈ R n k whereẽ k =x k − x k . Letẽ k be the k-th subcomponent ofẽ, which is the estimation error e =x − x. Now we obtain: For k = 1, . . . , q set: where k ( ) is given by (19) and D k ( ) is given: It is therefore easy to deduce that D k ( )C T η k C k = C k C T η k k ( ). Using the identities (22) and (30), into equation (29), it yields:ė Now, from the equations (18) and (27), and also using the identifies (29), one gets: Adding and subtracting the term δ k S −1 k C T k C kē k , one obtains the following error dynamics: From the observation gain K k (S k ) in (28), one has: which permits to rewrite the error dynamics: wherez k =η k − U kx k . From the equations (33) and (36), one obtains the time derivative ofz k , as follows: therefore one gets the following dynamic equation: Now, consider the following Lyapunov candidate function: where V 1,k (ē k ) =ē k T S kē k and V 2,k (ē k ) =z k Tz k . Firstly, one focuses on the term V 1,k (ē k ), differentiating V 1,k (ē k ) along the trajectories of the system: where S k is given by (26). Introducing the ODE Lyapunov equation (36) into (40), it yields: where σ k i = σ k 1 + (i − 1)δ k . In addition, considering Assumption A1-A2, i.e. the boundedness of ∂ϕ ∂x , one has: where Lφk comes from Assumption A2 and the χ k,i l,j 's are defined as in Lemma 1 in [31].
Remark 1: The complexity of the proposed algorithm depends on the size of the uncertainty, which can be reduced to a certain value through the parameter . Nevertheless, there is a trade-off between the sensitivity of the measurement noise and the convergence error. The sensitivity can be compensated by the observer immersed in the controller by means of the adjustment parameter µ, however, the uncertainty is limited to a certain value.

V. SIMULATIONS
To validate the performance of the novel observer-based IDA-PBC, some simulation tests have been performed. The observer given by (25)-(27) is applied to the quadrotor dynamical model (18). An IDA-PBC controller with gravity compensation is used to stabilize the vehicle's position and attitude dynamics.

A. PERFORMANCE IN PRESENCE OF MEASUREMENT NOISE
In order to highlight the performance of the proposed control law in the presence of the different noise signals, two experimental results are provided. In the first test, the noise signal w(t) is a Gaussian noise with a variance equal to 0.0005, this results in the noisy output measurements plotted in Figure 2. In the second test, a noise signal w(t) is given by 0.155 sin(20t), the perturbed trajectories are plotted in Figure 3. In both simulations, the uncertainty is ε(t) = ε 1 = ε 2 = ε 3 = ε 4 = 0.1 cos(20t), this uncertainties can be attached to unmodeled dynamics, such as wind frictions and structural vibrations, as [25].
The performance of the novel observer-based IDA-PBC control in the presence of the Gaussian noise is presented in Figure 4, in addition, the behavior of the design in the presence of the sinusoidal noise signal is illustrated in Figure 5. Note that in both experiments the proposed  observer-based control provides a good trajectory tracking in presence of uncertainties and disturbances as the measurement noise (Gaussian noise and Sinusoidal noise). Considering all these non-negligible effects, the novel approach will make easier the practical control implementation. It is also important to highlight that the performance of the proposed design has a slight performance degradation in the presence of the Gaussian noise, as shown in Figure 6, this degradation relates in particular to high-frequency measurement noise, in order to reduce this effect, some works add a low-pass filter of high order [29]. However, this may be impractical to compute for high-order systems as the quadrotor. Furthermore, these designs address only the SISO uniformly observable systems, this is not the case for quadrotor dynamics (MIMO non-uniformly observable system).

B. COMPARISON TO A STANDARD DESIGN
In order to compare the behavior of the standard and proposed control design in the presence of the high amplitude noise signals, a comparison test is performed. For this test, the noise signal w(t) is a sum of a Gaussian noise with a variance equal to 0.001 and a signal given by 0.25 sin(20t), this results in the noisy output measurements plotted in Figure 7 (proposed design) and Figure 8 (standard design). Moreover, this test considers the same uncertainty of the previous tests. Figure 9 shows that the proposed controller can reach the reference in a very short time in the presence of the high-amplitude  noise signal, even in the mixed noise (Gaussian noise and Sinusoidal noise). Moreover, as it can be seen in Figure 10,  the proposed algorithm outperforms the standard approach of [14], which assumes that the output measurements are noise-free. On the other hand, Figure 11 makes evident that the quadrotor cannot reach a near-zero steady-state error with the traditional observer-based PD control. Furthermore, it is clear that there is high chattering phenomenon in the standard control law, in Figure 10.

VI. CONCLUSION
In order to avoid the heavy design effort and ensure the robust control strategy in the presence of adverse effects as disturbance, uncertainty, and noisy measurements, it has been presented a new observer-based IDA-PBC control for a quadrotor. The original idea of the proposed strategy of the observer-based control is that it is able to guarantee a near-zero steady-state error, in the presence of relatively large uncertainties and disturbances, as measurement noise and unmodeled dynamics of the quadrotor (wind frictions and structural vibrations). The features of the proposed strategy were further validated with simulation tests for a quadrotor, which show a remarkable improvement of the sensitivity of the estimator subject to measurement noise and thereby achieving a reduction of the steady-state error. The behavior of the proposed observer-based IDA-PBC control for small uncertainties and disturbances tends to be similar to the behavior of the typical approach, i.e., high-gain observer-based PD control without uncertainties and disturbances. Future work includes extending the scheme for the case of sampled output measurements and delayed output measurements, as well as the combination of both. Moreover, it will consider an IDA-PBC design with total energy-shaping. FRANCISCO-RONAY LÓPEZ-ESTRADA received the Ph.D. degree in automatic control from the University of Lorraine, France, in 2014. Since 2008, he has been a Lecturer with the National Tecnológico Nacional de México, IT Tuxtla Gutiérrez. He has led several funded research projects, and the funding for these projects comprises a mixture of sole investigator funding and collaborative grants. He is the author/coauthor of more than 40 research articles published in ISI-journals and several international conferences. Furthermore, he collaborates with European universities as the University of Lorraine, University of Paris 2, University of Stavanger, Polytechnique University of Catalonia, and among others. His research interests include fault diagnosis and fault-tolerant control based on convex LPV and Takagi His research interests include robotics, renewable energy, and identifying mechanical faults with motor current signature analysis. VOLUME 9, 2021