A Nonlinear Filter for Efficient Attitude Estimation of Unmanned Aerial Vehicle (UAV)

Autonomous estimation of the state is of key importance in UAVs, as the measurement systems may experience faults and failures. Thus estimation techniques must provide estimates of the most important variables used in the control algorithms for safe, autonomous, unmanned flights. In this paper, a filter with low computational complexity for attitude estimation of a quadrotor UAV is introduced, with a model suitable for Fault-Tolerant Observation. The new filtration method, called the Square Root Unscented Complementary Kalman Filter (SRUCKF), is based on the commonly-known Kalman Filter (KF) in its nonlinear version, namely the Square Root Unscented Kalman Filter (SRUKF). The fundamental equation of the KF is modified so that the complementary feature of the filter is exalted. The new filter introduces characteristics that are analyzed on the basis of its application in quadrotor state estimation. Finally, the results are compared to an ordinary filter of the same type (using the Unscented Transformation). The presented studies indicate that the newly derived filter (SRUCKF) handles strong nonlinearities and gives results similar to those obtained from the SRUKF. Furthermore, it introduces lower computational burden, as the undergoing process uses diagonal matrices in its crucial places. In the paper, the estimation algorithms are tailored to a quadrotor UAV (Crazyflie 2.0), for which a quaternion-based model is proposed. The contribution of the paper lies in a Kalman-based novel state observer and its application in Fault-Tolerant Observation (FTO).


Introduction
Unmanned aerial vehicles have been an area of development. These flying vehicles have a wide range of applications and offer tremendous potential. Although UAVs are intentionally designed to fly without an operator, their autonomy is still strongly limited (among other reasons due to the use of imprecise low-cost sensors, which causes uncertainty in measurements of the current position and orientation). Therefore, rising UAVs to a higher level of autonomy is one of essential tasks of modern robotics. In order Wojciech Giernacki wojciech.giernacki@put.poznan.pl 1 AISENS, ul. F. Lubeckiego 23A, 60-348 Poznan, Poland 2 Institute of Control, Robotics and Information Engineering, Electrical Department, Poznan University of Technology, Piotrowo 3A, 60-965 Poznan, Poland to strengthen the robustness of the robot's measurements system, state observers are used.
In the past years, estimation in robotics was considered as a process of assessing unmeasurable variables of the studied object. At first, linear models were used in observers. This was possible thanks to the Luenberger observer [1], as well as the Kalman-Bucy filter [2]. Later on, a nonlinear version of the Kalman-Bucy filter was developed, namely the Extended Kalman Filter (EKF). The EKF works on a basis of the Kalman Filter theory, with the difference that in each step the nonlinear representation is linearized and then the Jacobian matrix is used as the system matrix. Although the EKF has been used for more than 40 years [3] and has been applied successfully in many works [4][5][6], it is an imprecise method. It suffers from approximations made during the linearization procedure. The EKF approximates the mean value and covariance up to the first order in the Taylor series. In the case of highly nonlinear problems it does not reflect the statistical values properly. It is also believed that in extreme cases it exhibits divergence and can be hard to tune when dealing with significant nonlinearities [7]. The problem of filter divergence and linearization errors could be minimized in the next generation of filters which was initiated by the Second Order Extended Filter. The Second Order EKF uses linearization with accuracy up to the second term in the Taylor series. This allows precise estimation, however, the cost is high, as this filter uses the Jacobian and Hessians in the process. This complicates the implementation. Moreover, the second order EKF introduces higher computational complexity, which makes it less attractive. In the middle of the nineteen century, an interesting alternative for the EKF was developed: the Unscented Kalman Filter (UKF).
The Unscented Kalman Filter was firstly proposed in [8], as a solution to the EKF problems. The main concept behind the UKF is the Unscented Transformation (UT). During the Unscented Transformation, a set of carefully chosen points (vectors), called sigma points, is passed through the nonlinear function. The sigma points are chosen so that the mean value and covariance of the transformed function can be properly captured. The UT uses the resampling technique, so the statistic is calculated on the basis of the set of points. Technically, if the noise distribution is Gaussian, the UKF provides approximation up to the third order, and if the input is non-Gaussian, the filter still holds the approximation accuracy to at least the second order of the Tylor series [9]. The UKF has been applied in various works, focusing mainly on attitude estimation [10], position estimation [11], or model parameters estimation [12]. The algorithm is given in Appendix A.1. It uses the square root of the covariance matrix and the Cholesky decomposition. The square root is satisfied with a semipositive matrix, however, the Cholesky factorization needs a positive-definite matrix to operate properly. This may lead to significant instability issues. Moreover, the basic square root of a given matrix A involves searching for B: which is time consuming. Taking into consideration Lemma 1 and Lemma 2 (Appendix B), the following is obtained: where Q is orthogonal, while D is diagonal, which is proved in Appendix B. Matrices Q and D are found in the process of Schur decomposition [13]. This method can be accomplished by using QR decomposition in an iterative manner. At least a few (k) iterations are needed. Assuming that the the QR is O(nl 2 ), for any diagonalizable matrix M ∈ R n×l and n l, and taking into account that A ∈ R n×n , the overall complexity will be O(kn 3 ) (technically k 8). On the other hand, the Cholesky factorization is O(n 3 /6). Finally, the UKF algorithm complexity is O(kn 3 ).
The complexity, as well as the stability issues due to round-off errors in the Cholesky factorization, prompted researchers to develop a better implementation of the UKF. An interesting solution was given in [9], namely the Square Root Unscented Kalman Filter (SRUKF). The idea behind the SRUKF is to create an efficient tool for unscented filtration, where the square-root of the matrix is replaced by a stable square root form propagated throughout the filtration process. It means that the square root is directly updated, without its straight calculation. To clarify this, the SRUKF algorithm is given in Appendix A.2. The algorithm uses the QR decomposition and the Cholesky update/downdate methods instead of square root and the Cholesky factorization. The Cholesky update is O(n 2 ) and the QR for that case (decomposed matrix ∈ R 3n×n ) is O(3n 3 ). The advantage of the SRUKF over the UKF can be clearly seen. The authors of the SRUKF claim that the method is numerically stable and guarantees semipositiveness of the square root of the covariance matrix [9]. The SRUKF did not, however, meet with enthusiastic reception. In fact, only a few works regarding the SRUKF were published in the first eight years after its disclosure. Later, the SRUKF was used in many applications, mainly in state estimation of mobile robots (for instance [14]).
Both algorithms (UKF and SRUKF) are suited for applications where the state and output functions are nonlinear, which is a general case. In many cases the output function h(...) is a linear combination of state elements. More often than not, the output vector uses the whole state vector. For these cases, the previously given methods can be tailored, yielding a better performance. Here, the authors propose a method which is based on the Kalman Filter theory, though it uses a modified fundamental equation. The method is suited for the particular case when the state and output vectors are identical, i.e.: In order to verify the proposed method, the problem of state estimation in a quadrotor UAV is studied. Although state estimation in UAVs might be considered as a separate research problem, it is highly important to show a nonlinear estimation problem and to propose tools handling the case. Among various estimation problems, the quadrotor state estimation is one of the most important research topics in the field of UAVs. In order to not get into details, only orientation will be studied in terms of estimation performance during normal and disturbed conditions. This often refers to fault-tolerant attitude estimation and faulttolerant observation [15,16]. Orientation estimation is the most common need which has to be fulfilled in all types of drones, as the provided data are necessary to fly (unlike position). Among many methods of attitude estimation, there are a few that are used in standalone devices called the Attitude and Heading Reference System (AHRS), and those which are employed in on-board computers as algorithms. In the AHRS, the Strapdown Inertial Navigation Systems (SINS) are used. In a basic form, an SINS is a combination of two basic types of sensors: accelerometers and gyroscopes. By taking into account these two, angular velocity as well as angular position can be obtained. Although an SINS system made of a threeaxis accelerometer and a three-axis gyroscope can provide data to calculate attitude and velocities, this is insufficient to calculate heading. In order to estimate full orientation, at least magnetometer data must be incorporated. There are many algorithms and nonlinear techniques that can be used for attitude and position estimation on the basis of data from an SINS supported by magnetometers and GPS. The most popular are Kalman Filters (KF) [17], Complementary Filters (CF) [18], and Particle Filters (PF) [19]. They are, however, not necessarily proper algorithms in the case of attitude and position estimation of multirotors. As the environment of the on-board systems of UAV multirotors is rather hostile for sensitive MEMS sensors and permeated with distorted magnetic fields, special methods must be used for undisturbed estimation. Moreover, the estimation process must be robust and valid even in the case of sensors failure/fault.
In the paper [20] the authors present methods of state estimation. The most interesting is the AHRS-related part, where a new filter design is shown. The authors have focused on solving the Wahba's problem, which constitutes the merit of the orientation observer. The solution indeed proved to be reliable, even though when dynamic or magnetic disruptions occur. However, that is not due to the complementary filter but to the proposed model. Thus the complementary filter can be simply replaced with the Kalman Filter. Researchers care about low complexity and case of tuning, especially when it comes to UAV's on-board implementations, but when one is searching for a minimalvariance solution, the Kalman Filter is a tool of choice. That is especially important when the estimation model is coupled with control, i.e. control has a direct influence on estimation and at the same time the estimation results are used in a control.
In this paper, authors propose modified SRUKF method called SRUCKF. The idea is to take into account a model of the quadrotor UAV and the AHRS data. Quadrotor UAV dynamics can be reflected as a nonlinear model with given parameters (w k ). By providing a closed-loop model, where the input is the set position (u k ), the model can be used to get the robot's attitude (x k ). By making a fusion of the AHRS data and the model state, the robot state can be observed. This simple idea was tested in attitude estimation with the Unscented Kalman Filter in [21] and with the Adaptive Square-Root Unscented Kalman Filter (ASRUKF) in [14]. In contrast, the new observer is compared to the standard SRUKF, and it is proved that the proposed method is similar in terms of overall estimation performance, with lower computational burden. The proposed method is shown to deal with serious AHRS errors due to a simulated fault/failure. The formulated Fault-Tolerant Observer can be used in sensor Fault-Tolerant Control tasks as well as in a robust state estimation framework. The provided method, i.e. the SRUCKF, the model, and the estimation architecture fall into the FTO and FTC discipline [22][23][24].
Results from the experiment are presented against the reference data gathered from the VICON motion capture system for the Crazyflie 2.0 unmanned aerial vehicle (Fig. 1). Details about Crazyflie 2.0 are widely presented in [25].
The most important, practical/useful advantage of the approach proposed here, in relation to the solutions available in commercial mid-range price quadrotors (like the popular PixHawk controller or XSens AHRS unit with stand-alone state estimation solutions already implemented in their firmware), is the openness and flexibility of the solution when the drone has to perform an unconventional task in very difficult conditions under variable load. Most of commercial units are still characterized by insufficient level of security due to insufficient quality and speed of the delivered position and orientation estimates, operating on various sensory sources (AHRS unit, GPS, laser and vision sensors) during autonomous flights between outdoor and indoor conditions. The UAV community is currently devoting more and more attention to research issues in the area of fault-tolerant observers [16] and fault-tolerant control [23,24].
For example, let's consider mission no. 3 of the prestigious Mohamed Bin Zayed International Robotics Challenge (MBZIRC2020) in which we deal with the use of data fusion from various sensors to operate a UAV autonomously both indoors and outdoors, carrying a payload (fire extinguisher and blanket = flight with variable mass). The use of budget AHRS units here (usually supported by classical Kalman filtration) and lowlevel flight controllers (like the mentioned PixHawk) in combination with a basic GPS module is unfortunately insufficient to ensure fast and safe flights. The need to develop methods of precise estimation and control has already been demonstrated by the previous edition of the MBZIRC competition (mission with precise and fast autolanding on a moving car [26] and "treasure hunt" [27], i.e. autonomous collection of metal discs by a group of UAVs). In the area of effective fault-tolerant estimation and fault-tolerant control (especially in visual estimation and control) there are many works published that draw inspiration from computational intelligence [15], as well as from the above-mentioned analytical estimation techniques.
The work presented here opens the door to the next level, in which we devote special attention to the problem of estimation in conditions of limited sensory information. Due to the multitude of issues and the potential volume of work, the authors decided to divide the results into a series of four articles dedicated to: UAV dynamics modeling [28], effective estimation methods, fault-tolerant estimation and control, as well as fusion of measurement and video data.
This article is organized as follows: in the second section the new method is derived, next, in the third section the estimation problem is explained, with a model of the quadrotor given in the quaternion form. In next two sections the tailored state observer is shown and state estimation results are presented. Finally, in the last section conclusions are drawn and a summary is given.

The New Observer
As has already been said, the new observer proposed in this paper is based on the Kalman Filter theory, however, its main equation is modified. We will start the new filter derivation process with a linear case. For the KF, the fundamental equation, given as follows: is now replaced with the following, complementary one: It is highly important to note that the weight matrix W k can be found in an optimization process, or in a similar way as it is done in the original filter. When the dimension of the output vector is equal to dimension of the state (dim (y k ) = dim (x k )), Eqs. 7-8 represent a linear state-space model of the plant. It can actually be considered as an estimation problem in itself, since the input u k is acting on the plant and at the same time the vector y k is being measured.
where v k ∈ R n×1 is a zero mean white Gaussian process noise vector with covariance Q k , while o k ∈ R m×1 is a zero mean white Gaussian measurement noise vector with covariance R k . Since the model remains unchanged, the a priori covariance is the same as in the original KF: on the basis of: P k−1 can be simplified to Eq. 13: The a posteriori error is derived on the basis of Eq. 6: while the a posteriori covariance is as follows: Thus: Note that P k and P − k are symmetric, thus P k = P T k and P − k = P − k T . By calculating P k it is possible to determine the optimal filter gain W k . The procedure requires minimization of the trace of P k . By applying Lemma 3 (Appendix B), the derivative of P k can be calculated, yielding the following: which finally gives: With a given W k , P k is rewritten to the Joseph form: Having already the covariance P k and gain W k , the Complementary Kalman Filter equations are presented: This can be compared to the KF with C k = I n×n : These two basic filters, i.e. the proposed one (20)-(24) and the classic one (25)- (29), are used in the further derivation, as they constitute an analogy. The original covariances of the UKF, i.e. P xy and P yy , can be denoted as follows: The new observer has two major differences (based on the filters analogy), i.e.: and which finally results in the following equations of the Unscented Complementary Kalman Filter (UCKF): Applying the same rule to the SRUKF, the SRUCKF equations are obtained: where i ∈ (< 1 : n >).
The new observers have the same effectiveness as the UKF and SRUKF, however, with better computational properties. 1 The major advantage of the Complementary Kalman Filtr lies in the relation P xy = R k , which guarantees a diagonal form of P xy . The SRUKF and SRUCKF will be studied in terms of errors and robustness. As it was stated before, neither the UKF nor the UCKF will be used, as these observers have worse properties compared to their Square-Root versions.

UAV Model
In the paper, the new observer SRUCKF and the standard SRUKF are studied in quadrotor UAV fault-tolerant state estimation. The problem of state estimation is one of the most important, especially when the on-board measurement system introduces serious errors. Moreover, the UAV's navigational modules may undergo faults and failures, which is the a issue in autonomous operation. The idea is to minimize the influence of these errors, faults, or even failures, while preserving functionality and performance of the control algorithms. This can be done via estimation, where data from more than one source of information are used. In one of the available solutions, at least two sensors of the same type are used. In another solution, a mathematical model as well as one sensor can be used. Here, the second idea is applied, along with a mathematical model of a quadrotor UAV. The basic model has two main parts: the orientation and the position, which together form the pose of the robot or the full state. Here, the model (adapted from [28]) is based on the Newton-Euler equations for rigid body in 3D space and has the following layout: where the torques are as follows: 1 Note that the CKF is equal to KF when W k = I − K k , so the same UCKF and SRUCKF while the forces: Nomenclature: ω -angular velocity, I -inertia matrix, τinput torque (from propellers), τ g -gyroscope torque due to the robot's frame and propulsion system, τ d -torque from aerodynamic effects, τ t -friction torque, ν -linear velocity, m -UAV mass, f -input force, f g -gravitational force, f c -Coriolis force, f d -drag force, f t -friction force, p iposition of the i-th propeller, I w -inertia tensor of motor and propeller, ω w -sum of propeller velocities, R -rotation matrix, h g , b w , c w , b w 1 -respectively: friction coefficients. Note that the gravitational acceleration g depends on the reference system in which the positioning is done. The aerodynamic drags, i.e. flapping, induced, translational, and profile, acting on the i-th rotor, are denoted as For the sake of clarity, further details will not be given, although the reader can find more information on aerodynamic drags in [29]. The studied quadrotor has a classic frame and is equipped with four rotors, its scheme is given in Fig. 2.
As it can be seen, two coordinate systems are introduced. The first one, namely the Body Frame {BF}, is fixed to the robot, while the second one is the inertial reference system {EF}. {EF} is ENU, where the X axis is pointing East, Y is aligned with the North, and Z is pointing Up of the Earth geographic coordinates. This reference is popular, as it provides an intuitive insight. Since g ∈ {EF}, it is given as follows: The transformation matrix R b e transfers any given vector from {EF} to {BF}, while its transpose R e b ensures the inverse action, i.e. {BF} → {EF}. R b e is based on the Euler/Tait-Bryan angles or quaternions, although different representations can be also used. In the paper, quaternions are used, as they do not suffer from singularities or Gimbal-Lock issues.

Fig. 2 Quadrotor scheme
The same functionality is achieved with the quaternion q and its conjugate q * : or conversely: Note thatṽ ∈ R 4 , and in the case of quaternions for any vector v its extended version is equal toṽ = 0 v T .
The notation with Kronecker products (⊗) is inconvenient, especially when performing multiple operations on a single vector. In order to make the quaternion notation clear and obvious to the reader, the authors decided to use the notation given in [30]. A basic explanation on the notation is given in Lemma 4 (Appendix B). By taking this into account, it is possible to express (71) as follows: or Eq. 73 as: The orientation model can be constructed on the basis of the angular velocity equation given in quaternion representation [31]: where: which can be also given in the matrix form: The angular acceleration in {BF} is a derivative ofω: which, substituted to Eq. 56, yields: The same can be applied to the force equation, where the linear acceleration given in {EF} is as follows: Note that in Eqs. 80, 81 the inertia matrices have the form proposed in [31]: Since all vectors are extended with additional dimension, cross products are replaced by Kronecker products.

Tailored State Observer
The state observer is model-based. In the first phase it uses model data, and during the update phase it incorporates measurements. Here, a quaternion-based model is used, together with an AHRS. Either a closed-loop model or an open-loop model may be used, although in the paper the research is narrowed only to the closed-loop model, as only the set point of position is available. In the open-loop model some physical quantities, e.g. rotor velocities, are needed, and in the case of the closed-loop model the set value of the real robot must be provided. Estimation with the closed-loop model can be realized as shown in Fig. 3. The scheme is proposed by the authors and will be studied in the next section. The most important idea behind the estimation scheme given in Fig. 3 lies in two control loops: one on the real robot and one using the robot model. The input is the same for these two loops and it is the set position. The control loop (either the robot The attitude control utilized a simple formula for the error, i.e.: which was proposed by [32]. q set is calculated on the basis of set position. When controlling in {EF}, the set quaternion is derived on the basis of the set Euler angles set : where R s is determined on the basis of the chosen location of {BF}, here as follows: Note that ψ s = ψ. The vector u s is composed of axial values used directly by the position controllers. Finally, the signal mixer, which is used in the motor controllers, has to be described. Normally, proper rotor velocities are increased and decreased, however, due to the is equally added to all rotor velocities, forming the relation given in Eq. 89.
where P is a permutation matrix which depends on the location of rotors in {BF}. Similar to u s , u k are the axial set values provided by the attitude controllers.
Having covered the control tasks of the model (ie. from trajectory planning in the high-level control layer, through position and attitude control, to rotational speed control of UAV's motors in the low-level layer), observers can be derived. The control is performed according to Fig. 4. As it has already been stated, the observers use a model with controllers, forming a closed-loop system (see Fig. 3). The input for the model is the set position, while the output may consist of the full state, as well as of its selected elements. When dealing with a multirotor UAV, the most wanted are the orientation and position. When only the AHRS system is available, the position cannot be measured, though it can be simulated in the control process (via estimation using a model). Having such estimation data and the model, one is able to simulate a drone flight. On the other hand, the orientation is provided by the AHRS and it can be corrupted during faults and failures. That is why state estimators are used. Here, the authors decided to observe the quaternion, its first derivative and angular/linear velocity. For this purpose the state and output vectors are set as follows: The functions f x k , u k and h(x k ) have the following forms: where: The equations above are suitable for the SRUKF/UKF observer, however, since the authors use also the SRUCKF observer, a modified state vector and output vector should be proposed. For any filter where m = n, the following can be adopted: for which the model function (system model) is given as: These two observers (SRUKF, SRUCKF) will be compared in terms of accuracy and robustness. Note that the state vectors are different, however, the aim is to find a precise tool for state estimation where only the attitude is important, so even after adding linear velocity or position the attention is still focused on orientation. The authors consider these two observers as possessing minimal complexity. Moreover, these observers are easily expandable with additional state elements, which makes them flexible.

State Estimation Results
In experiments the drone was sending the sensory data to the base station where also the data from the VICON motion capture system were gathered (Fig. 5). The VICON was used as the ground truth. The drone is a modified Crazyflie 2.0, for which BLDC motors were installed instead of the classic DC ones.
The research involves two main sources of information: the model and a low-cost AHRS (InvenSense MPU-9250), which is EKF-based (providing only quaternion and angular velocity, its main idea is given in [4]). The trail data gathered in the experiment allowed a basic AHRS statistic to be determined. The results are presented in Table 1. As it can be noticed, the estimation results are rather poor, however, they are sufficient for basic maneuvers, and a great majority  of low-cost drones have similar AHRSs. This system is extended with additional observers which were described in the previous section. It is, however, important to introduce some basics of controllers and observers settings. In the drone, six PID controllers are used, for which eighteen parameters have to be set. Observers, either the SRUKF or the SRUCKF, have 3 parameters to be set, i.e. α, β, and κ, which are set according to [9]. Additionally, observers have the covariance matrices Q k and R k , where the elements on their main diagonals (variances) have to be set according to the state variables and measurements. In the case of the SRUKF observer, the elements of Q k , namely q ii are as follows: For the SRUCKF observer, q ii are given in Eq. 101.
In the case of R k , its diagonal elements r ii are equal for all i.
In the optimization process, the Particle Swarm Optimization (PSO) method is used, where the cost function is chosen so that the errors of attitude and position are taken into account: The results of optimization of the SRUKF and the control settings are presented in Table 2, while the results of state estimation are given in Table 3.
If one compares the stand-alone AHRS with the SRUKF, it can be seen that the results are significantly in favor of the SRUKF observer. The mean error has been reduced by at least one degree, while the standard deviation is lower for each axis. The maximum error is similar and could not be reduced. The trail data are presented in Fig. 6. As it can be seen, the AHRS estimate suffers from bias in Euler angles/ quaternions, which is significant. When these data are combined with the model prediction, it is possible to better imitate the drone's behavior. That was proved in the obtained results. Since the model was used in a closed loop with the observer in the feedback, the controller settings had to be optimized. The obtained optimization results are given in Table 2. As it can be noticed, the orientation controllers were simplified to the PD scheme, as the K i parameters were small enough to be treated as zero.
In the next experiment the SRUCKF (the new observer), whose state function was given in Eq. 99, is studied. The process of controller settings tuning was repeated. By using the same cost function C f , the results given in Table 4 were obtained. The SRUCKF estimation results are shown in Table 5. The orientation is estimated better if compared to the SRUKF, however, the cost is bigger, mainly because of the position, for which the set value could not be followed so well as in the case of the SRUKF. Since r ii is lower than in the case of the SRUKF, the AHRS data are treated as less important in the estimation process. What is interesting, the integral parts of the position controllers have smaller influence on the control process (Table 4). This stems from the observer construction, which does not have position and linear velocity included in the state vector. It also proves that the controller settings depend on the observer state structure, even when some of the state variables are not directly used in the control process. The trial data with the SRUCKF estimation results are given in Fig. 7.
These two observers provide a good estimate of orientation. The controller settings and the observer structures are different, however, the cost functions are similar. On the basis of the presented data it cannot be said which one would be better in a UAV application. Since the authors postulate fault-tolerant estimation in the paper, it is desirable to test their robustness against errors. For this purpose, an AHRS failure will be simulated. Among many possible scenarios of AHRS faults/failures, only one will be studied. In most of the AHRSs, the process is driven by external interruptions. They are often chosen to be based on a particular sensor. When such sensor does not respond, a timeout procedure is invoked, so the process can be handled without one or more sensor data values. Although these emergency procedures can cope with some temporary issues, they will eventually fail if sensor reset a constant value. Although many other possible AHRS faults/failures exist, these are the most common ones. In order to verify observer robustness, the AHRS was prepared to simulate a failure by sending a constant signal. The estimation results are given in Table 6, as well as in Table 7 and Fig. 8. The result are similar to the previous case, when the sensory data were fault/failure-free. It means that, for the observer settings used, the algorithms immune to sensory data disruption. Both the SRUKF and SRUCKF have proved to be robust. This feature is, however, not inherited with a given observer. It occurs normally when the Kalman Filter settings strongly limit the trust in correction data, i.e. sensory data, which is clearly the case. It also implies that in the case when the sensory system is of industrial or tactical grade, this feature will not be so evident. For such systems adaptive observers have to be used. The robustness test reveals that the observers are equally good when applied in the UAV. It also shows that with the given framework (combining the ARHS and the model data) an FTO can be obtained, making it possible to tackle down sensory system failures or faults.

Conclusions
In the article, two observers have been introduced. The first one is based on the SRUKF, and the second one is based on the modified Kalman Filter called SRUCKF. For these two observers, different state functions were proposed. In all studied scenarios, the closed-loop scheme was used, where the input was the set position. Thanks to the proposed control structure and the precise robot parameters it was possible to follow the set position similarly as it is done in a drone. The authors have focused on the orientation, which is normally estimated by the AHRS. By introducing the closed-loop model, another source of information could be exploited. As it was proved, both observers can operate on the basis of the model and AHRS data. Moreover, during emergency situations the observers can operate incessantly while being only fed with drone model data, without valid correction data -FTO.
One of the assumptions in the research concerns the algorithms complexity, or the computational burden. In the case of the proposed observers, the state vectors have the same dimension, however, the output vectors differ. In the SRUKF m = 7, while for the SRUCKF m = 11 (dim (y k ) = m). Since the inverse matrix is needed, whose complexity is in general O(m 3 ), the SRUCKF will clearly not be optimal. However, in the same operation, the matrix multiplication is done, for which a diagonal matrix is used instead of a symmetric one. On that basis one can notice some advantages of the SRUCKF over the SRUKF. According to [9], the additional optimization mechanisms can be added to SRUKF/SRUCKF. This refers mainly to the matrix inversion. Since in the Kalman gain equation the upper triangular matrix S y is used, the standard procedure of matrix inversion can be replaced by backward substitution. This reduces the inversion complexity to O(m 2 ). After applying the backward substitution method, the SRUCKF can compete with the SRUKF.
In the paper, robustness of the presented methods was tested. As it was proved, both observers can cope with temporary sensory faults or even permanent failures. It is, however, important to stress the role of observer settings. In the experiment, a drone with a low-cost AHRS was used. It simply induced low trust for correction data, and thus it implied observer robustness. This, however,, has to be changed when an industrial or tactical grade sensor is used. For such systems, an adaptive mechanism must be used, which will be subjected to future investigations.
Lemma 4 Kronecker product of quaternions in a matrixbased notation. Let us assume that three quaternions: α, β, and γ are given in the following relation: By introducing the matrix forms of α and β it can be written that: where: In Eq. 150,ã andb are skew symmetric matrices: Publisher's Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. He is an author or co-author of more than 60 publications in peer-reviewed international conferences, articles in impacted journals and 1 book. He serves as a reviewer for several impacted journals as well as a program committee member of relevant international conferences. His scientific interests are focused around the issues of UAVs, especially robust and adaptive control, optimization techniques, as well as data fusion from sensors.
Andrzej Królikowski graduated from the Poznan University of Technology (1968). He received the Ph.D. and D.Sc. degrees in control engineering and robotics from the same University in 1974 and 1995, respectively. Till 2017 he was a full professor at the Poznan University of Technology. He is an author or co-author of more than 100 publications (at international conferences, articles in impacted journals, and books). His research interests include control theory, adaptive systems and identification.