Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter

This paper presents the Quaternion-based Robust Adaptive Unscented Kalman Filter (QRAUKF) for attitude estimation. The proposed methodology modifies and extends the standard UKF equations to consistently accommodate the non-Euclidean algebra of unit quaternions and to add robustness to fast and slow variations in the measurement uncertainty. To deal with slow time-varying perturbations in the sensors, an adaptive strategy based on covariance matching that tunes the measurement covariance matrix online is used. Additionally, an outlier detector algorithm is adopted to identify abrupt changes in the UKF innovation, thus rejecting fast perturbations. Adaptation and outlier detection make the proposed algorithm robust to fast and slow perturbations such as external magnetic field interference and linear accelerations. Comparative experimental results that use an industrial manipulator robot as ground truth suggest that our method overcomes a trusted commercial solution and other widely used open source algorithms found in the literature.


Introduction
With the rise of small flying vehicles, also known as drones, small and inexpensive attitude and heading reference systems (AHRS) have populated the market. For such systems, it is common to estimate the orientation by combining information from magnetic, rate and gravity (MARG) sensors, also known as inertial measurement unit (IMU), usually composed of micromechanical (MEMS) three-axis gyroscope and accelerometer and a three-axis magnetometer [1]. The standard approach for attitude estimation is to compute the three components of inertial orientation by integrating the gyroscope measurements, and use the gravity projection and heading angle estimated by the accelerometers and magnetometers to correct the angles computed with the gyro. Although theoretically simple, naive implementations of this approach may not be precise because magnetometer measurements are easily influenced by ferrous material in the vicinity, and accelerometers measure not only the gravitational direction but also its linear acceleration. In these cases, it is difficult to dissociate magnetic field perturbation and linear acceleration from both the magnetic field of the earth and gravity to compute the attitude accurately, which can lead to poor estimates [2,3]. Since accurate attitude estimation is a crucial task for a variety of applications, such as human motion tracking [4,5], augmented reality [6], satellite control [7] and navigation and control of aerial [8], and sub-aquatic vehicles [9], the main objective of this paper is to present a solution that deals with external in internal sensor disturbance and interference. This paper extends the results of our conference paper [10]. We provide a more detailed mathematical description of our method and algorithm given measurements from a MARG sensor. The proposed algorithm has low computational cost and is able to reduce the effect of the magnetic disturbance. The problem of this algorithm is that the orientation estimated using accelerometers suffers the influence of magnetic disturbances due to the coupling in the gradient descent algorithm used. In Ref. [29], quaternion measurement is computed as the composition of two algebraic quaternions, mitigating the influence of magnetic distortion. Adaptive gains are used to reduce the estimation error during high dynamic motion. Instead of using adaptive factors, in Ref. [30], an air data sensor (ADS) is used to compensate for the linear acceleration disturbance in the MARG sensor. Although interesting, this solution requires a minimum system velocity, making its application limited.
In this scenario, we propose an adaptive algorithm based on UKF for attitude estimation using quaternion representation. In contrast to EKF, UKF-based approaches can better handle the high order nonlinear terms that can appear when the attitude error is large. The unit norm constraint of the quaternion is ensured by the use of a rotation vector parameterization. Differently from quaternion error approximation used on USQUE, this parameterization better handles the unit quaternion properties, such as the Riemannian metric [31]. The main difference between our work and classical approaches, such as MEKF and USQUE, is that it handles online the time-varying measurement uncertainty. For this, the covariance matching approach is used to update the observation covariance matrix online. Although CM can usually estimate well the measurement covariance matrix, the presence of outliers can damage the estimation. To minimize the effect of measurement outliers, we propose to combine the adaptive filter with a Hampel identifier, which compares the median deviation and the median absolute deviation (MAD) to identify an outlier. Our approach only uses the MARG sensor as an information source, and does not need extra information, such as ADS. Thus, the main contribution of this paper is the Quaternion-based Robust Adaptive Unscented Kalman Filter (QRAUKF) algorithm for attitude estimation. This algorithm is robust to fast and slow perturbations on both accelerometers and magnetometers and, to the best of authors' knowledge, is the first one with such characteristics that precisely and consistently represent the attitude using quaternions. The proposed algorithm is tested with real experimental data collected from a MARG sensor. The performance of the proposed algorithm is confronted against the non-adaptive UKF, the open source algorithm based on complementary filter proposed in Ref. [32] and the commercial algorithm embedded in the MARG device used in our experiments, which was executed using a manipulator robot for validation purposes. A Matlab implementation of the algorithm is freely provided by the authors.
The rest of this paper is organized as follows. A problem formulation is presented in the next section. The theoretical basis for our contribution is in Sections 3 and 4. The proposed algorithm is presented in Section 5 and is experimentally evaluated in Section 6. Finally, conclusions are presented in Section 7.

Problem Statement
This paper addresses the problem of attitude estimation of a rigid body moving in a three dimensional space. This problem basically consists in determining the three degrees-of-freedom orientation information of the body, independently of its current velocity and/or acceleration. Among the common representations for attitude, we chose to use unit quaternions, since it is a free-of-singularities and compact representation. To estimate the attitude, we assume that rigid body is attached to inertial and magnetic sensors, namely 3-axis accelerometer measuring acceleration in the three main body axis, a x,m , a y,m , and a z,m ; 3-axis magnetometer measuring the magnetic fields b x,m , b y,m , and b z,m ; and 3-axis gyroscope measuring the angular velocities ω x,m , ω y,m , and ω z,m . The accelerometers measure the gravity acceleration, which is indeed the part used for attitude estimation, along with all other accelerations to which the body is subjected, which act as perturbations for the movement. On the other hand, magnetometers measure the magnetic field of the earth, useful in the estimation process, and other unknown fields around the body. Measured accelerations and magnetic fields are contaminated with zero mean random noise. Angular velocity information, besides random noise, is also subjected to a time varying bias. Thus, our problem is to estimate the unit quaternion e = [e 1 , e 2 , e 3 , e 4 ], given the vector of measurements [a x,m , a y,m , a z,m , b x,m , b y,m , b z,m , ω x,m , ω y,m , ω z,m ], which is corrupted with noise and external disturbances, as mentioned before.
We solve the previous problem using a modified version of the Uncented Kalman Filter (UKF). A block diagram of our solution is shown in Figure 1. All blocks of this figure will be discussed in the following sections. We start by addressing attitude representation using unit quaternions and by presenting a new Unscented Transform (UT) for quaternions in the next section.  Figure 1. The architecture of the quaternion-based robust adaptive Kalman filter. In the left, the MARG sensor provides the measurement information. The filtering algorithm uses the gyros measurement ω m ∈ R 3 in the forecast step. The UT block is used to propagate the accelerometer a m ∈ R 3 and magnetometer b m ∈ R 3 measurements through a nonlinear function, computing a unit quaternion, used as a pseudo-measurement in the robust noise estimation and data-assimilation steps.

Unit Quaternion Operations
Unit quaternions form a four-dimensional algebra over the real numbers and can be used to parametrize the rotation group SO(3). The set of unit quaternions, denoted by H 1 , form a group under multiplication operation but not under addition operation [17]. This group is topologically a 3-sphere, denoted by S 3 , embedded in the R 4 .
The unit quaternion can be represented as e = (v, n) ∈ H 1 , in which e = 1.
Here v ∈ R is the real part and n ∈ R 3 is the imaginary part. Given a rotation θ and the unit vector w, the corresponding unit quaternion is e = cos θ 2 , sin θ 2 w . The inversion unit quaternion operation is equal to its conjugate, given by e −1 = e * = (v, −n). The product ⊗ between quaternions is defined as e a ⊗ e b v a v b − n T a n b , v a n b + v b n a + n a × n b , in which × denotes the cross product [33]. A vector v ∈ R 3 can be rotated by a unit quaternion e, which is given by (0, u) = e (0, v) e * , where u ∈ R 3 is the rotated vector [11].

Euclidean Tangent Space and Rotation Vector Parametrization
The group S 3 is a Riemannian manifold, whose elements can be represented in a three-dimensional Euclidean tangent space T e S 3 . Many operations are defined in the Euclidean tangent space, such as the empirical mean and covariance. Furthermore, there are direct and inverse mappings between the manifold and its tangent space, S 3 ←→ T e S 3 , with exponential and logarithm functions, respectively.
Let e = (v, n) be a unit quaternion and r = θw be a rotation vector representing a rotation θ about the unit axis w. The unit quaternion to rotation vector mapping, called logarithm mapping, is given by [17]: where the quaternion antipodal ambiguity e = −e is treated by checking the signal of v.

Sum, Subtraction, and Weighted Mean Operations
We define now the operations of sum, subtraction, and weighted mean for unit quaternion states. The difference between e a and e b ∈ H 1 is defined as Similarly, for the Euclidean vectors ξ a and ξ b ∈ R n , this operation is giving by ξ a − ξ b . The sum of a unit quaternion e a ∈ H 1 and a rotation vector r ∈ R 3 , is defined as In Euclidean space, this operation is ξ a + ξ b . Lastly, the weighted mean operation for a set of unit quaternions E = {e i }, i 1 . . . n w , is defined asê where W = {w i } is a set of corresponding weights. The quaternion meanê ∈ H 1 can be computed in a closed form by [34] where M ∈ R 4×4 , and the quaternion mean is the eigenvector of M corresponding to the maximum eigenvalue. Iterative algorithms can also be used, as the gradient descent algorithm presented in Refs. [10,11].

Quaternion Unscented Transform
The unscented transform (UT) is the main core of UKF. The UT approximates the meanŷ ∈ R m and its covariance P yy ∈ R m×m of a random variable y obtained from the nonlinear transformation y = h(x 1 , x 2 , c), where x 1 ∈ R n 1 and x 2 ∈ R n 2 are random variables with meanx 1 ex 2 and covariance matrices P x 1 x 1 ∈ R (n 1 −1)×(n 1 −1) and P x 2 x 2 ∈ R n 2 ×n 2 , respectively, and c a known deterministic variable. In addition, the random variable x 1 is composed by a unit quaternion part x 1,H and a unconstrained Euclidean part x 1,E ; thus Now, we define the augmented state vectorx ∈ Rn as wheren = n 1 + n 2 , as well as the augmented covariance matrix Pxx ∈ R (n−1)×(n−1) The UT is based on a set of deterministically chosen samples known as sigma points (SP). The sigma points X j ∈ Rn −1 and the associated weights w j , j = 1, . . . , 2(n − 1) can be chosen as where X j is the jth column of matrix X ∈ R (n−1)×2(n−1) , (·) 1 2 is the Cholesky square root operation, and [1] 1×2(n−1) ∈ R 1×2(n−1) is a row vector with elements equal to one. Notice that the columns of the covariance matrix Pxx can be seen as a perturbation variable, where the unit quaternion part is parameterized as a rotation vector, which means that the covariance matrix is defined in the tangent space, hence then − 1 dimension. The SP (9) can be partitioned as where X x 1 ∈ R (n 1 −1)×2(n−1) and X x 2 ∈ R n 2 ×2(n−1) . Then each sigma point X j is propagated through h: ∈ R n y is the jth column of the matrix Y ∈ R n y ×2(n−1) . From (12), we obtainŷ, P yy and P x 1 y aŝ At this point it is important to mention that Equations (9) and (13)-(15) differ from the one in the standard UT transform because they consider the quaternion operations previously defined in this section.
From now on, for notation simplicity, we define the quaternion unscented transformation as the function UT(·) comprising the set of Equations (9)-(15) as: wherex and Pxx are given by Equations (7) and (8), respectively.

Mathematic Modeling
This section describes the discrete time dynamic model used by the filtering algorithm presented in this paper.

Kinematic Model of Attitude
Assuming that angular rates ω k ∈ R 3 , measured by a 3-axis gyros from the input vector u k of the dynamic system, the discrete-time attitude model is given by [15] vec where vec (·) : H 1 → R 4 is an operator that takes the four coefficients of the unit quaternion and stacks them in a 4-vector, k denotes the discrete time, and , We assume that u k = ω k ∈ R 3 is corrupted with random noise and bias terms, modeled as To directly use the measured inputs in (17), bias terms and random noise are estimated and subtracted from the measurement. Then, A more general model for gyrometers is u m,k = (I 3×3 + S) u k + β k + q u,k , where S ∈ R 3×3 denotes the matrix of scale factors and misalignment. This model is more appropriate for in-lab calibration [2]. Thus, in this paper, we assume that scale factors and misalignment have already been determined and compensated, and only the biases are estimated [35].
Due to bias, which may vary in time, the attitude estimated based on gyros measurements may suffer a temporal drift. To minimize this problem, bias terms β k are modeled as a random walk process, where q β ∼ N [0] 3×1 , Q β ∈ R 3 and are jointly estimated with the other system states, yielding a Equations (17) and (19) compose the process model, which can be compactly presented as where f denotes a nonlinear function of previous state x k−1 , with input u k−1 , and process noise

Observation Model
The observation model relates the components of state vector x k with the output measurement vector y k ∈ H 1 , defined as y k e m . Measurements are corrupted by random errors and modeled as e m = e k ⊕ r k , where r k ∼ N ([0] 3×1 , R k ) ∈ R 3 is the measurement noise parameterized as a rotation vector. Therefore, the observation model may be written as In this paper, the measured acceleration a m,k = a x a y a z T ∈ R 3 and magnetic field b m,k = b x b y b z T ∈ R 3 are used to compute the unit quaternion e m ∈ H 1 . Assuming normalized measurements such that a m,k = 1 and b m,k = 1, the unit quaternion representing the body attitude can be computed as [13,29]: where e * m (a m , b m ) is the conjugate of e m (a m , b m ) and where l m,k = l x l y l z T such that(0, l m,k ) = e * acc (0, b m,k ) e acc . Because these equations are nonlinear, the unscented transform, given in Equation (16), is used to propagate the measured acceleration error r a,k ∼ N ([0] 3×1 , R a ) ∈ R 3 and magnetic field error r b,k ∼ N ([0] 3×1 , R b ) ∈ R 3 through Equations (22)- (24). Then, where diag(A, B) forms a block diagonal matrix with matrices A and B, and h ab (a m , b m ) ∈ H 1 is given by Equation (22).

State Estimators
We assume that our dynamic system is modeled by the nonlinear Equations (20) and (21) in which, ∀k ≥ 1, the known data are the measured output y k and input u k−1 . It is also assumed that process noise q k−1 ∈ R n q and measurement noise r k ∈ R n r are mutually independent with covariance matrices Q k−1 ∈ R n q ×n q and R k ∈ R n r ×n r , respectively. The state estimation problem aims at providing approximations for the meanx k = E[x k ] and covariance P xx that characterize the a posteriori probability density function (PDF) ρ(x k |y 1:k ).
Due to the nonlinear characteristics of the model, our proposition is to use as the basis to our approach the unscented Kalman filter (UKF) [36]. In the standard form of the UKF, two problems arise when it is used to estimate attitude: (i) the UKF pertains to Euclidean systematization, therefore containing sum and weighting operations, which are not defined for unit quaternions; (ii) the output measured noise r k can have time-varying statistical properties, which can, in the worst case, lead to diverging estimates. Regarding (i), most of the issues are solved if the modified unscented transform presented in Section 3.3 is applied in the place of the standard one, as shown in Section 5.1. The solution of (ii) is our core contribution. We consider two events that may change the statistical properties of measured noise: a dynamic event, such as linear accelerations that mask the gravity vector projection measured by accelerometers; and a external influence, such as a ferromagnetic element that disturbs the Earth's magnetic field measured by the magnetometers. The rejection of these perturbations is addressed in Sections 5.2 and 5.3.

Quaternion-Based UKF
The UKF algorithm presented in this section is based on the ones shown in Refs. [11,17], which are slightly modified to encompass direct unit quaternion measurements and multiplicative noise in the process. Henceforth, the notationx k|k−1 indicates an estimate of x k at time k based on information available up to and including time k − 1. Likewise,x k indicates an estimate of x k at time k based on information available up to and including time k. Let the process noise be partitioned as q k−1 is the noise nonlinearly related to the state vector and q 2,k−1 ∈ R n x −1 is the linear partition of noise. To improve the numerical stability of the filter, additive noise is considered for all states [37]. Given these definitions, the modified quaternion-based unscented Kalman filter (QUKF) forecast step is given by where ν k is the innovation. The augmented state vectorx k−1 ∈ Rn and the corresponding covariance matrix Pxx k−1 ∈ Rn ×n are respectively given by The state estimate and error covariance matrix are updated using information from y k in the data-assimilation step, given by:

Adaptive Covariance Matrix
The uncertainty of measurements in the UKF is represented by covariance matrix R k , which is usually constant. However, the measurement uncertainties can be time-varying. We then propose the use of innovation ν k to adjust the measurement covariance matrix online through the covariance matching (CM) approach [18].
Based on the assumption that the observation covariance matrix R k is constant during a sliding sampling window with finite length N, the basic idea of CM is to make the innovation ν k consistent with its covariance E[ν k ν T k ] P yy k|k−1 . Notice that the innovation pertains to the three-dimensional Euclidean tangent space. Thus, the covariance of ν k is estimated as based on the last N innovation samples as Notice that, the UKF (see Equation (29)) approximates the covariance by E[ν k ν T k ] P yy k|k−1 + R k , whereP yy k|k−1 ∑ 2(n−1) j=1 w jỹj,k|k−1ỹ T j,k|k−1 . Then, R k can be estimated bŷ To avoid negative values due the subtraction operation in Equation (35), negative values inR k are replaced by their correspondent value in the nominal covariance matrix R 0 .

Outlier Rejection
Outliers are spurious data that contaminate the statistical distribution. The contaminated measurements deviate significantly from the normal observations, which directly reflects in the innovation value ν k , and, consequently, in the covariance estimated by CM.
The Hampel identifier [38] is an outlier identification method that is reported as extremely effective in practice [39]. Based on this approach, our contribution is to compute a gain λ ∈ R n r ×n r to be used as a multiplier that reduces the outlier influence in the estimation of the covariance matrix and also on the Kalman gain. This gain is a diagonal matrix, wherein each the diagonal is defined as where s i = 1.4826 med{|ν j,i − med{ν j,i }|} is the median absolute deviation (MAD), n σ is the number of standard deviations (confidence region) by which the innovation sample must differ from the local median, med is the median operator, {·} is a moving window with size N, j k − N + 1 . . . k is an index for each element of the moving window, and i is the index of each element of the innovation vector.

Quaternion-Based Robust Adaptive Unscented Kalman Filter
By combining Equations (35) and (36) with the QUKF Equations (26)-(33), we then obtain a three step algorithm that we call quaternion-based robust adaptive unscented Kalman filter (QRAUKF). The first step is the forecast step, which is given by Equations (26)- (28) and (30). The second step is the robust noise estimation given by Equation (36), the estimate covariancê and (29). The third and last step is the data-assimilation step, which is given by Equations (31) and (33), andx

Experimental Results and Discussion
In this section we compare the performance of the proposed QRAUKF algorithm to the QUKF, the complementary filter (CF) proposed in Ref. [32], and the commercial algorithm embedded in the MicroStrain 3DM-GX1 IMU. We implemented QRAUKF using Matlab (Our code is available at https://bitbucket.org/coroufmg/raukf_cm).
Actual data was collected at 40 Hz from the IMU, which was mounted on the end effector of a Comau Smart Six manipulator, used to perform controlled movements and to provide accurate orientation information. Figure 2 illustrates our setup. To set QUKF and QRAUKF, we have assumed that the covariance matrix Q 1,k−1 ∈ R 3×3 is diagonal with elements related to the angular rates measured by the gyros. This matrix was estimated as σ ω = [0.4584 0.3724 0.4927] T deg/s. The additive noise of process was represented by the diagonal matrix Q 2,k−1 ∈ R 6×6 . This matrix is related to the attitude, parametrized as a rotation vector, and the bias terms of the gyros. The standard deviations were empirically set as σ v = 57.3 × 10 −20 3×1 deg and σ β = 57.3 × 10 −9 3×1 deg/s, for attitude and bias terms, respectively. The measured acceleration and magnetic field are propagated through the nonlinear function represented by Equations (22) , respectively. The nominal covariance matrix of measurements R k is computed by the UT. The sliding window size of RAUKF was empirically set to be N = 20 samples, which represents a period of 0.5 s during which the noise covariance is assumed to be constant, and the confidence region n σ = 3 standard deviations. CF has two parameters, the gain that quantifies the gyro measurement noise, set as β = 0.007, and the gain that quantifies the bias terms, set as ζ = 0.01. These values follows the authors recommendations [32]. The standard deviations σ ω , σ a , and σ m were estimated from experimental data. For this, we have used a window of approximately 20 s. During this calibration process, the MARG sensor was kept stationary (steady-state behavior). The tuning parameters were estimated before performing the main state estimation experiments, during which they remain unaltered. Observe that the measurement covariance matrix R k is updated online, in contrast with the process covariance matrices Q 1,k−1 and Q 2,k−1 , that also remain unaltered. Although we used a simplistic parameter estimation approach, the parameterization seems to be appropriate, even for different experiments.
Five disturbance scenarios were evaluated: (i) abrupt and (ii) slow varying magnetic disturbances; (iii) linear accelerations; (iv) individual axis rotation about the origin; and (v) simultaneous axes rotations about the origin. The last two experiments, scenarios (iv)-(v), suffer linear accelerations due to the lever arm between the end effector of robot and IMU (Videos showing the experiments are found at: https://goo.gl/mtFSqG).

Magnetic Field Distortion
Heading estimation is performed by monitoring the Earth's magnetic field with the magnetometer. Due to the proximity of ferrous or magnetic materials, the magnetic field can be locally distorted, which causes inaccurate estimates of heading angle. In our first experiment, the magnetic brakes of the robot manipulator are turned on and off a few times, thus causing an abrupt variation in the magnetic field that is perceived by the magnetometers. Due to the shaking caused by the release of the brakes, some spikes of acceleration also appear. Figure 3 shows the acceleration and magnetic field measurements, and the attitude estimation error for each axis. The disturbance periods are highlighted. Observe that the QUKF is more sensitive to outliers in the acceleration and magnetic field measurements, converging quickly to wrong estimates induced by inaccurate measurements. This behavior is expected, once QUKF does not have any treatment for abnormal measurements. In contrast, QRAUKF, CF, and 3DM-GX1 algorithms reject the outliers. Table 1 shows the Root Mean Square Error (RMSE) for this experiment in the column called "Abrupt magnetic". Observe that QRAUKF performs better than other algorithms, with the smaller RMSE. In contrast, QUKF yields the largest RMSE indices.
In a second experiment, the magnetic field was artificially and slowly disturbed with a magnetic material. Figure 4 shows the acceleration and magnetic field measurements, and the attitude error for each axis. This kind of perturbation is usually difficult to detect and can damage the estimation. Notice that QRAUKF is less sensitive to the slow varying abnormal measurement. CF yields the worst results as shown by RMSE in Table 1, in the column called "Slow magnetic 1". Figure 5 shows the bias estimation in y and z-directions. Observe that the abnormal behavior of the magnetic field affects the bias estimates of angular rate in all directions for QUKF and CF, which in turns does not happen with QRAUKF.      In a third experiment, the magnetic field was also artificially and slowly disturbed with magnetic material. Although similar to the previous experiment, in this one the magnetic field is continually disturbed during 30 s. Figure 6 shows the measurements and attitude estimation error for each axis. In addition to magnetic disturbance, during the experiment we accidentally touched the manipulator with the magnet and observed that the accelerometers and gyros also measure abrupt changes just before 10 s and 20 s. Figure 7 shows angular rate measurements for this experiment. All algorithms, except QUKF, are robust to outliers in acceleration and magnetic field measurements, however, as can be seen by the error results, all algorithms have large peak errorsφ andθ. These large errors are caused by the outliers in the angular rate measurements, that are not handled by the algorithms.
In spite of these outliers, notice that QRAUKF shows the best result, as can be seen in Table 1    . Angular rate ω y and ω z , respectively, measured by the gyros for the third experiment, scenario (ii).

Linear Acceleration Disturbance
Roll φ and pitch θ angles are computed by the projection of the gravity vector, which is measured by the accelerometer. However, the accelerometer measures the linear body acceleration together with the gravity vector, which masks the gravity vector observation. Thus, the linear acceleration disturbs the observation of φ, θ, and consequently the heading angle ψ.
To test the behavior of the algorithms against the perturbation of linear velocities, the manipulator executed independent translational movements in each axis. Figure 8a shows the accelerometer measurements. We observed that even when the movements are being executed separately in each axis, linear accelerations appear in all axes. This is probably due to a small angle in the link joining the IMU and the robot end effector. Figure 8b-d shows the attitude error. Table 2 shows the values of RMS for this and other experiments. Notice that the QUKF provides the worst results. In contrast, QRAUKF yields the best RMSE indexes and the smallest peak-error for heading angle. We also observe that after 30 s, the attitude error for QRAUKF grows due to the longer time exposed to linear acceleration perturbation. In fact, the QRAUKF estimates tend to converge to the value induced by the perturbed measurement after a period of time.

Rotations about the Origin
In our last two experiments, presented in Figures 9 and 10, rotations about the origin in each axis separately and simultaneously were performed. In these cases, estimates are influenced by linear accelerations that appear due to a lever arm between the IMU and the robot end effector. Figures 9a and 10a show the actual movement performed by the manipulator. Again, the proposed algorithm yields the best results, as shown by Table 2. Notice by Figure 9 that QRAUKF and 3DM-GX1 algorithm have similar errors, however, QRUKF converges faster to measurement after the perturbation finishes. The poor performance of KF and CF is due to bias estimates that are influenced by linear acceleration.

Conclusions
In this paper, a quaternion-based robust adaptive unscented Kalman filter for orientation estimation was presented. The algorithm ensures the unit norm of quaternion in all algorithm steps without forcing a normalization. The logarithmic map of unit quaternions is used to parametrize the error quaternion. This parameterization allows us to perform operations in Euclidean space and then use existing approaches to adapt the measurement covariance matrix and detect outliers. Due to the nonlinear nature of this transformation, unscented transform is used to compute the measured quaternion.
The proposed algorithm was compared to a nonadaptive version of UKF, a complementary filter, and commercial algorithm embedded in the IMU. Some experiments were performed to verify the performance of the algorithms in situations where distorted magnetic field and linear accelerations exist. The proposed algorithm shows the best RMSE results in all situations tested, and the smallest peak-error for linear acceleration disturbance. In addition, the proposed algorithm accurately estimates the gyros bias terms. Although in this paper we only show attitude estimation, the proposed methodology can be used along the standard UKF equations to estimate the full state vector of vehicles. The same ideas for disturbance rejection can also be extended for other vehicle states, once the sum and subtraction operations defined in Section 3.2 are the usual operations for Euclidean states.

Conflicts of Interest:
The authors declare no conflict of interest.