A Novel Fuzzy-Adaptive Extended Kalman Filter for Real-Time Attitude Estimation of Mobile Robots

This paper proposes a novel fuzzy-adaptive extended Kalman filter (FAEKF) for the real-time attitude estimation of agile mobile platforms equipped with magnetic, angular rate, and gravity (MARG) sensor arrays. The filter structure employs both a quaternion-based EKF and an adaptive extension, in which novel measurement methods are used to calculate the magnitudes of system vibrations, external accelerations, and magnetic distortions. These magnitudes, as external disturbances, are incorporated into a sophisticated fuzzy inference machine, which executes fuzzy IF-THEN rules-based adaption laws to consistently modify the noise covariance matrices of the filter, thereby providing accurate and robust attitude results. A six-degrees of freedom (6 DOF) test bench is designed for filter performance evaluation, which executes various dynamic behaviors and enables measurement of the true attitude angles (ground truth) along with the raw MARG sensor data. The tuning of filter parameters is performed with numerical optimization based on the collected measurements from the test environment. A comprehensive analysis highlights that the proposed adaptive strategy significantly improves the attitude estimation quality. Moreover, the filter structure successfully rejects the effects of both slow and fast external perturbations. The FAEKF can be applied to any mobile system in which attitude estimation is necessary for localization and external disturbances greatly influence the filter accuracy.


Survey on Attitude Estimation
The microelectromechanical systems-based (MEMS-based) relative localization problem is a recent topic, which has been widely investigated in many areas including robotics and control [1][2][3][4][5][6][7][8], healthcare and rehabilitation [9][10][11], consumer electronics mobile devices [12][13][14], and automated driving and navigation [15][16][17][18], both in industry and in scientific research. Independent from the application, accurate and robust attitude estimation is a crucial task to be solved, especially if the results are to be incorporated into unstable closed-loop systems, such as the control algorithms of mobile robots and unmanned aerial vehicles (UAVs) [1].
AEKF was proposed in [45] with similar filter efficiency. The adaptive strategy identified both static and dynamic body motions. Moreover, the effect of external acceleration was suppressed through filter gain tuning. The attitude estimation problem during sports activities was addressed in [46], where the proposed EKF considered the model uncertainty of active acceleration. Experiments highlighted the robustness of the approach, especially when large accelerations were present during the tests. In [47], the maneuvering target tracking problem was addressed and the application of both General Regression Neural Networks (GRNN) and an additional maneuver detector algorithm was proposed for the state estimation of manoeuvring objects. Moreover, a comparison of the GRNN-based neural filter and KF for target movement vector estimation was presented in [48,49], where the GRNN-based approach was characterized by superior estimation performance only during steady motions. In [50], a fuzzy inference system was proposed to tune the noise covariance matrix of the EKF based on the filter innovation sequence through a covariance-matching technique. The experimental results showed that the fuzzy rule-based adaptive strategy effectively improved the estimation accuracy with respect to the standard EKF algorithm. In [51], an adaptive analytical algorithm was presented for the determination of UAV orientation angles. The algorithm employed both MARG and GPS-based correction channels; moreover, an UAV maneuver intensity classification method was implemented to increase the orientation estimation performance.
Recent studies have proposed the use of unscented KF (UKF) over EKF [52,53], and stated that UKF-based approaches better deal with the high-order nonlinear terms of large attitude errors. Attitude estimation has been solved with computationally efficient geometric UKF [53], where a new formulation of the UKF algorithm was proposed in [52] to maintain fast and slow variations in the measurement uncertainty. The latter algorithm was augmented with both an adaptive strategy to tune the covariance matrices on-the-fly and an outlier detector to reject the effects of external disturbances. An industrial manipulator robot was used to conduct the experiments, where the algorithm provided superior performance over the standard UKF and Madgwick filters. Recent developments have considered the MARG as a non-Gaussian stochastic system and developed maximum correntropy KF (MCKF) for attitude estimation [54,55]. These algorithms employed the MC criterion, instead of the minimum mean square error, to estimate the state of the system corrupted by non-Gaussian impulsive noises. However, the comprehensive case study provided in [56] has not highlighted the superior state estimation performance of the MCC-based techniques in non-Gaussian noise environments.
Based on the methods discussed above, it can be concluded that the ultimate attitude estimation quality is determined by three main factors: 1. The first impact is related to the flexibility of the implemented algorithm (i.e., the observation models, equations defining the filter dynamics, and noise models jointly define the algorithm). 2. The filter performance heavily depends on properly selected filter gains (i.e., noise covariance matrices). In general, the statistics of system noise cannot be determined; moreover, external disturbances cause radical measurement noise during attitude realization, which make the assumed noise models inappropriate. As a result, the filter parameters are usually selected based on both experimental and engineering intuition, which result in a compromise between the accuracy and filter dynamics. To enhance the filter performance, numerical optimization-based filter tuning has been performed [1,14,40,57]. 3. The papers above show that the common methods used to deal with external disturbances (dynamic motions and magnetic perturbations) either work by the application of intelligent adaptive strategies that on-the-fly modify the vector observation methods, filter gains, and covariance matrices; or the compensation is maintained with additional dynamic models that well-mimic the effects of the external forces and magnetic fluctuations.

Contribution of the Paper
This paper addresses the robust attitude estimation problem for mechatronic systems (robots) characterized by fast dynamics, unstable equilibria, and/or mechanical difficulties (e.g., the driving mechanism backlash). For these type of systems, reliable state estimation is both an essential and crucial task to be solved, as the unstable dynamics are stabilized in closed-loop with a control algorithm, in which the stabilizing system inputs are calculated based on the estimation results. If the state estimation contains significant errors, then these control signals will drive the system out of equilibrium to unwanted states, which may eventually damage the system and its environment [1,42].
The aforementioned discussion highlights that providing both reliable and robust attitude estimates, especially for extreme dynamic situations, remains an important issue. For this problem, our paper proposes a novel qAEKF, in which new methods are employed to measure the external disturbances and their effect is suppressed with adaptation laws described with fuzzy logic-based IF-THEN rules. The results show that the proposed methods significantly improve the robustness of the state estimation, both in static and extremely vibrating and accelerating environments. Moreover, to the author's best knowledge, no study has yet investigated the attitude estimation problem in such dynamic environments. The basis of the proposed filter structure was presented in [1], where the techniques were validated for one-dimensional attitude estimation using a linear KF. That investigation showed promising results, thereby motivating us to extend the estimation problem to the complete orientation based on MARG systems. The novelties of the paper are summarized as follows.
1. Formulating a novel quaternion-based FAEKF structure that incorporates the magnitudes of vibration, external acceleration, and magnetic perturbation by a sophisticated heuristic knowledge-based fuzzy inference machine to provide robust attitude estimation in both static and dynamic environments. 2. Designing a 6 DOF test platform which enables both the execution of various dynamic (vibrating and accelerating) behaviors in the three-dimensional space and the measurement of true attitude angles along with the raw MARG data. An additional part of the test environment is a novel magnetic perturbation algorithm. This test environment contributes to the successful evaluation of state estimation quality. 3. Performing numerical optimization-aided tuning of filter parameters based on the collected training measurements in the test environment. 4. Providing a free-to-use Robot Operating System (ROS) package that enables both the generation of MARG-based measurements and the testing of filter performances. We made this package publicly available on our website [58], with the aim of helping other laboratory teams with both performing and developing similar experiments.
The proposed approaches can be advantageously applied in such mechatronic systems where accurate attitude determination is crucial for the closed-loop dynamics; moreover, where external disturbances are frequently present, due to fast maneuvers, collision, or unstable dynamics.
The remainder of the paper is organized as follows. Section 2 gives an introduction to quaternion representation and highlights the important relationships. In Section 3, the stochastic models of MARG sensor arrays are discussed and a suitable EKF formulation for attitude estimation is described. Section 4 presents the fuzzy adaptive strategy in detail, in which external disturbance magnitudes are measured with three novel methods; additionally, a sophisticated fuzzy inference machine is employed to manipulate the noise variances consistently. Section 5 introduces the test bench which was designed for estimation quality evaluation, the optimization-aided tuning of filter parameters, and the experimental results of the proposed approaches. Finally, in Section 6, the conclusions and recommendations for future studies are discussed.

Quaternion-Based Attitude Formulation
Let E and S denote the earth and sensor frames, also called the global non-moving inertial and local mobile frames, respectively. These frames can be defined with the conventional North-East-Down (NED) configuration often applied for robotic applications [3,5,42]. Namely, the x-axis points north and y is directed east, whereas z completes the right-handed coordinate system by pointing down in the inertial reference frame (see Figure 1). Additionally, the origin of the right-handed sensor frame is attached to the center of mass of the moving body, where the x-axis points forward and the y-axis is directed to the right of the body. The mapping between these frames E and S is described by a rotation matrix as where E x and S x denote the 3 × 1 vector observations in the earth and sensor frames, respectively. A quaternion representation provides an effective way to both formulate the aforementioned rotation matrix and describe the attitude of the coordinate frames in three-dimensional space [59]. The advantageous structure both provides fast computation (compared to DCM) and completely avoids the well-known singularity problem of Euler angles (also known as the gimbal lock problem) [60]. The unit quaternion formulated by the four-dimensional vector E S q ∈ R 4 , E S q = 1 describes the attitude of frame E relative to frame S as a rotation by an angle µ about the unit vector e = e x , e y , e z T , which represents the rotation axis in S. This rotation quaternion is interpreted as E S q = cos µ 2 , e T · sin µ 2 T = (q 0 , ) T , where q 0 and = (q 1 , q 2 , q 3 ) T denote the scalar and vector part terms, respectively. Co-ordinate transformation is performed by the non-commutative quaternion product denoted by ⊗: In Equation (2), E S q * = (q 0 , − ) T denotes the conjugate quaternion that describes the attitude of frame S relative to frame E (i.e., the inverse rotation is formulated as E S q * = S E q). Moreover, E x and S x indicate the quaternions associated with the vector observations by their augmentation with zero scalar parts (q 0 = 0) as x = 0, x T T . The rotation can be rearranged into the initial Equation (1) with the quaternion-parameterized rotation matrix where I 3 is the identity matrix of size 3 and [ ×] denotes the antisymmetric matrix of , defined for the vector cross product × x = [ ×]x as Let S ω = 0, ω x , ω y , ω z T denote the four-dimensional quaternion formed by the angular velocities about the x, y, and z axes in the sensor frame. The time derivative of the quaternion E S q represents the rate of change of attitude E relative to frame S, according to the vector differential equation where the matrix-vector product is indicated by the quaternion matrix Q (q). The attitude of frame E relative to S is obtained by integrating the quaternion derivative E Sq . Thereforeforth, the sub-and super-scripts are omitted, for the sake of simplicity.
The authors used the Euler angles for the quality evaluation of attitude estimation, as their interpretation is straightforward for the reader. Euler angles (including yaw, pitch, and roll) describe the attitude as a sequence of three rotations, where ψ, θ, and φ denote the rotation angles about the z, y, and x axes, respectively. The quaternion output provided by the analyzed filters was converted to Euler representation as follows.

Attitude Estimation with MEMS MARG Sensors
Each sensor of a MEMS-based MARG unit provides useful information of the instantaneous attitude; however, none of the sensors are capable of providing reliable attitude results alone. Gyroscopes measure angular velocities; therefore, gyroscope-based attitude realization is obtained through numerical integration, but both the temperature-dependent bias and noise contained in the measurements cause cumulative errors. An accelerometer measures the sum of gravitational and external accelerations. In stationary states, long-term stable attitude realization can be obtained based on the decomposition of the sensed gravity vector but, as external accelerations increase as a result of dynamic motion, the quality of attitude realization drastically deteriorates, making accelerometer-based realization highly unreliable. Magnetometers measure the geomagnetic field, which is used to determine heading information. However, the magnetic fluctuation of the environment caused by the perturbation of ferromagnetic objects highly disturbs the magnetometer output.
To provide reliable attitude estimation results, the individual features of each sensor are carefully addressed in the following.

Gyroscope Model
Let Ω k denote the raw measurement vector of a tri-axis MEMS gyroscope in the k th time instance. This measurement vector is composed of a 3 × 1 vector ω k of true angular velocities around the x, y, and z axes, a vectorω k containing the non-static bias terms, and a vector µ k of additive measurement noises. The imperfections of manufacturing results, in that the sensor model is extended with axis misalignment and scale factor errors, are represented by the 3 × 3 matrices M Ω and ∆S Ω , respectively. Moreover, the temperature sensitivity of the sensor makes the slowly varying bias vectorω k propagate as a random walk process characterized by a driving noise vector η k , and therefore [61] in the above measurement model, the rate noise vectors contain zero-mean white Gaussian variables for each axis (i.e., E [µ k ] = E [η k ] = 0) and the covariance matrices are defined as E µ k µ T l = Σ µ,k δ kl , Σ µ,k ≥ 0, and E η k η T l = Σ η,k δ kl , Σ η,k ≥ 0, where δ kl denotes the Kronecker delta. Gyroscope-based (gyro-based) attitude realization is obtained by numerical integration of the true angular velocity vector ω k in Equation (7). Common calibration procedures performed in laboratories allow for the determination and compensation of the scale factor and misalignment errors. This process exceeds the scope of this article; therefore, we assume that the compensation has already been performed (M Ω = I and ∆S Ω = 0) [22,41,62]. Based on Equation (5), the gyro-based attitude realization is given in quaternion form as where T s = 1/ f s is the sampling time. However, this method yields only short-tem accuracy, due to the presence of bias and measurement noise terms (ω k and µ k ) resulting in boundless drift in the attitude propagation.

Accelerometer and Magnetometer Models
The accelerometer and magnetometer sensors provide absolute reference observations, and therefore their measurements can be combined to determine the complete attitude of the sensor. The raw output A k of a tri-axis MEMS accelerometer consists of four main components: the gravitational and external acceleration vectors g k and α k measured in the sensor frame (S), the vector a 0 of bias terms, and the vector ν k of additive measurement noises. Additionally, the raw measurement vector H k of the tri-axis MEMS magnetometer model is composed of the true local magnetic field h k sensed in S, the sensor bias vector h 0 , and the measurement noise vector k : Similarly to the gyroscope model, Gaussian noises are considered in the aforementioned models; therefore, Beside the scaling and misalignment errors (∆S A , ∆S H , M A , and M H ), the magnetometer measurements are disturbed by magnetic soft iron and hard iron errors caused by the local environment, represented by the 3 × 3 matrix B si and the 3 × 1 vector b h , respectively. These model errors are determined via self-calibration procedures which address the time-invariant nature of the vector fields and map the distribution of the measurements on an ellipsoid [63][64][65]. We assume that the compensation has already been performed (therefore, h k := B −1 si h k − b hi ), the bias and scale errors are zero, and the misalignment errors are identity matrices.
If a mobile mechatronic system stays in stationary states (i.e., no external acceleration is performed; α k ≈ 0) and, moreover, if the local magnetic field is not perturbed by ferromagnetic objects, then the locally constant reference vectors can express the observations, with the help of the rotation matrix, as In the aforementioned configuration, the gravity vector is given as E g = (0, 0, 9.81) T , whereas the magnetic field vector is E h = (b cos (σ) , 0, b sin (σ)) T in SI units, where b and σ denote the magnitude of the Earth's geomagnetic field and inclination angle, respectively.
Let the components of an inertial frame in both S and E be expressed by constructing two triads of orthonormal unit vectors. The first triad is defined with the reference vectors in E aŝ The second triad is constructed with the observation vectors in frame S, wherê Based on Equations (10)- (12), first the measurement (observation) and reference matrices are formed, then the rotation matrix is determined as: The determined rotation matrix S E R (q k ) = r ij enables the calculation of the quaternion representing the attitude of the sensor frame: The aforementioned algorithm is the well-known TRIAD [22,66], which produces the raw attitude realization based on accelerometer and magnetometer measurements. The attitude realization, which is described by Equation (14), is denoted by q k,TRIAD = (q 0 , q 1 , q 2 , q 3 ) T and can also be considered as the sum of the real attitude characterized by the quaternion q k in the k th time instance and an additive Gaussian white noise, v k , which represents the effects of ν k and k from Equation (9) after the TRIAD output is evaluated: This algorithm is characterized by a simple and straightforward implementation and, therefore, it is a popular choice for raw attitude determination [2,3]. However, it has a disadvantage in producing large errors when dynamic conditions are present or external magnetism disturbs the sensor readings. As a result, if external acceleration is performed (α k = 0 → S A = R E g) or ferromagnetic materials distort the geomagnetic field ( S H = R E h), then the attitude realization becomes unreliable with drastically reduced accuracy. This implementation method does not include any explicit models of external disturbances. Instead, the effects of external disturbances are absorbed by v k in Equation (15); that is, the additive noise is characterized by a significantly larger noise variance in disturbed environments.

Attitude Estimation with Extended Kalman Filter
The MARG sensor-based attitude realizations described by Equations (8) and (15) are utilized in a sensor fusion algorithm, which both synthesize the individual advantages and features of each sensor and provides attitude results with higher reliability and accuracy. First, this sensor fusion algorithm utilizes the gyroscope-based realization to propagate the attitude results, then these results are updated with the most recent quaternion realization derived from accelerometer and magnetometer readings. This propagate-update mechanism provides both a smooth output and stability in the attitude results by compensating for the drift error generated in Equation (8). The fusion of the sensor models is executed with an EKF.
The EKF effectively combines the noisy measurements and dynamic model-based predictions; moreover, in a recursive filter structure, it provides an approximate maximum-likelihood state estimatê x of the stochastic nonlinear state-space model [22]. In fact, the filter linearizes the nonlinear dynamic model around the last estimated state vector using the Jacobian matrix and, for the linearized dynamics, the linear KF is utilized, which is an the optimal state estimator such that The mathematical models and statistical assumptions of MARG sensors, as introduced in the previous subsections, fully match the process and measurement equations of a stochastic nonlinear state-space model. Namely, the process model describes the quaternion propagation with both the discrete-time integrated angular velocities (Equation (8)) and the random walk process of the bias term (Equation (7)). Therefore, the dynamic model is defined with the 7 × 1 state vector x k = (q k ,ω k ) T , the 3 × 1 input vector u k = Ω k , and the 7 × 1 process noise vector represents the quaternion noise generated due to the gyroscope measurement noise µ k . For the sake of comprehensiveness and to foster a straightforward implementation, we give the full description of state propagation in Equation (16): According to Equation (15), the measurement model is characterized by a linear quaternion mapping. Therefore, it is formed with the 4 × 1 output vector z k = q k,TRIAD which provides the quaternion update as the TRIAD output, the measurement noise vector v k , and the output matrix H, as If the x (0) Gaussian vector in Equation (16) is known along with its mean and covariance matrix; that is, ifx then the MARG sensor models fully satisfy the stochastic hypothesis. Namely, the process and measurement noise vectors are zero-mean white Gaussian variables, x (0) is uncorrelated to w k and v k , and, moreover, where Q ≥ 0 and R > 0 are the process and measurement noise covariance matrices, respectively.
The EKF algorithm provides a suboptimal state estimationx k with minimized estimation error covariance. The state propagation, processing of the observations, and the covariance estimate update are performed through time and measurement update equations in the recursive filter structure; namely, the time update equations utilize the input variable u k , the state estimation and error covariance obtained in the previous step (x k−1 and P k−1 ), and the state dynamics f (x k−1 , u k ) to calculate the a priori state estimate (x − k ) and the corresponding error covariance (P − k ): in Equation (20), the Jacobian Φ is applied in the a priori covariance matrix update. To foster straightforward implementation, we give its full form as follows, The measurement update equations utilize the both the observation vector, z k (accelerometer and magnetometer-based attitude realization), and the measurement noise covariance, R, to correct the a priori state estimate. First, the Kalman gain matrix G k is obtained, then the state estimatex k and its error covariance P k are corrected. The a posteriori estimation results are obtained in the following steps.
The estimation performance of EKF is mostly determined by the noise covariance matrices Q and R. Unfortunatelly, in practice, these parameters (i.e., the statistical description of the state and observation noises) are not fully measurable (or require time consuming, complex, and extensive verification and validation procedures); especially in the case of MARG sensors, as the effects of both different noise sources and disturbances are represented with general noise vectors v k and w k in Equations (16) and (17). Generally, the parameters Q and R are tuned based on engineering intuition through trial-and-error analysis; however, that method yields only a compromise solution between the estimation accuracy and filter dynamics. To overcome this compromise solution, numerical optimization-based approaches have been proposed in our earlier work. The proposed method both allows for evaluation of the best possible (achievable) estimation quality and provides the optimized parameters which maximize the filter performance [1]. We recall this approach to find the optimized parameters of EKF in Section 5.

Fuzzy-Adaptive Strategy
The adaptive approach varies the noise variances, according to both the instantaneous dynamical behavior and external distrubances, thus providing filter performance superior to that provided by the standard EKF. The instaneous dynamics are characterized by the magnitudes of vibration and external acceleration of the sensor frame. Moreover, the adaptive strategy incorporates the magnitude of the distorted geomagnetic field as an external disturbance. The following subsections present the structure of the adaptive strategy, in which both novel measurement methods of external disturbances and the novel sophisticated fuzzy logic-based inference machine are implemented for the real-time tuning of the noise covariances.
The measurement methods in Sections 4.1 and 4.2 have been described in detail, with multiple examples and figures, in [1].

Measuring Vibration Magnitude
The system vibration magnitude is described by the oscillation frequency of the sensor frame. For estimation of the instantaneous oscillation frequency, gyroscope readings are utilized, as the sensors provides reliable angular rate measurements for both static and highly dynamic motions. The oscillation frequency is obtained by fast Fourier transform-based (FFT-based) evaluation of short angular rate measurement packets. Let L denote the length of these packets. Then, an oscillation frequency estimationf is calculated, in three steps, as follows.
and the output is given by 3. Obtain the oscillation frequency estimatef by finding the highest-intensity frequency component, such that where f thr denotes the maximum oscillation frequency the system is expected to be exposed to, while the threshold rate magnitude |Ω| thr cuts off the noise in the aforementioned evaluation.

Measuring External Acceleration and Magnetic Perturbation Magnitudes
The external acceleration magnitude is calculated based on the accelerometer measurements. The system stays in stationary states (non-accelerating mode) if the magnitude of accelerometer readings is approximately equal to the norm of the reference vector E g . Therefore, the external acceleration magnitude ∆α k can be calculated as the difference between the norms of S A k and E g in each sampling epoch. As the instantaneous difference does not provide an overall picture of the system dynamics, an accumulated measure is thus utilized to describe the external acceleration magnitude. The accumulated measureα ext is formulated as the integrated scalar external acceleration for a window of length L (see Equation (26)). This average external acceleration measure provides both useful and broad information of the instantaneous system dynamics.
The magnetic perturbation magnitude is characterized based on the evaluation of the difference between the norms of S H k (instantaneous magnetometer measurement at epoch k) and E h (reference magnetic field). If no magnetic disturbance is present, then the magnitude of magnetometer measurement is approximately equal to the norm of the reference vector. Otherwise, the magnitude of their difference gives an instantaneous measure of the perturbation magnitude. As it is difficult to draw conclusions based on this brief and instantaneous result at each epoch, similarity to the accelerometer readings, an accumulated measure, is thus applied to quantify the magnetic perturbation magnitudê h ext .ĥ Similarly to accelerometer and gyroscope sensors, the magnetic perturbation magnitude is determined by collecting data packets of length L from the magnetometer and computing the average magnetic field difference using Equation (27).

Fuzzy Inference Machine
The measuresf ,α ext , andĥ ext fully characterize both the instantaneous system dynamics and disturbance magnitudes. These results can be utilized in an inference system in which the noise covariance manipulation of the EKF is described according to the external effects. As a result, an adaptive strategy is established that (online) tunes the noise covariances as a function of the measureŝ f ,α ext , andĥ ext .
The relationships between the aforementioned measures and the EKF parameters are defined with fuzzy reasoning. Fuzzy logic does not require complex mathematical models from the system designer but, instead, it enables the implementation of deductions easily and effectively by using fuzzy sets and simple IF-THEN linguistic rules. Therefore, heuristic knowledge and a collection of deductions make such an inference system realizable. The fuzzy inference system is executed in three main steps: fuzzification determines the membership values of the crisp input variables, inference machine translates the heuristic knowledge and assigns a firing value to each fuzzy output, and defuzzification maps the fuzzy output to the crisp domain. The main parts of the algorithm are detailed in [67].
Observations related to the system behavior and human common-sense contribute to collecting the empirical IF-THEN rules (deductions) that define the fuzzy inference machine. In the case of attitude estimation with MARG sensors, the main deductions are as follows. • IF the sensor frame stays in stationary (non-accelerating and non-perturbed) mode, THEN a well-chosen ratio between the noise covariances Q and R yields satisfactory state estimation performance.

•
As the external disturbance effects are absorbed by the measurement noise v k in Equation (17), IF vibration, external acceleration, and magnetic perturbations disturb the MARG-based attitude realization, THEN the measurement noise covariance R should be increased according to the intensity of the measuresf ,α ext , andĥ ext (i.e., higher noise variance characterizes the attitude realization q k,TRIAD with higher uncertainty).
The overall FAEKF structure is depicted in Figure 2, where a three-input one-output fuzzy inference machine executes the online tuning of noise variances. The inputs of the fuzzy system are the measuresf ,α ext , andĥ ext , whereas weighting factors, denoted by K R , are output weights for the R parameter (i.e., the adaptive strategy varies the measurement noise covariance matrix in each epoch k as R k = K R,k R). The ranges of the input variablesf (Hz),α ext (g), andĥ ext (normalized unit, nu), as well as the output variable K R , were selected based on our earlier research results in [1]. Three Gaussian membership functions cover each input range, where the magnitudes off ,α ext andĥ ext are characterized by Z (zero), S (small), and B (big) fuzzy sets. The output ranges were covered with seven singleton consequents (K 1 , · · · , K 7 ), which represent the scaling magnitudes. Both the applied membership functions and fuzzy inference system properties are depicted in Figure 3. The fuzzy surfaces expressing the relationships between the crisp inputs and outputs are depicted in Figure 4.
Accelerometer: A k Gyroscope: Ω k Time update:   A sophisticated inference system was implemented, where the initial deductions described above were expanded into 27 rules. These simple IF-THEN linguistic rules completely describe the scaling of noise variances, according to the magnitudes of the external acceleration, vibration, and magnetic perturbation. The implemented rule base for K R is summarized in Table 1. Two examples describe the interpretation of the implemented inference system, as follows: 1. IF the oscillation frequencyf is zero (Z) and the external accelerationα ext and magnetic perturbationĥ ext magnitudes are big (B), THEN a fairly large scaling factor (K R = K 5 ) is applied for the measurement noise covariance. This collocation of the system state means that the observation is expected to have rather large uncertainty and, therefore, the algorithm relies more heavily on the state propagation (left side, second row, second column). 2. IFf is small (S) and theα ext andĥ ext measures are close to zero (Z), THEN a smaller weight of K R = K 2 is applied for R. Therefore, the algorithm considers the observation with higher reliability and maintains the correction of the state propagation by processing the measurements with higher significance (middle, first row, first column).
The crisp scaling factor is computed by weighted average-based defuzzification of the fuzzy output, in three steps: 1. Fuzzification of the observation vector χ = f k ,α ext,k ,ĥ ext,k ∈f ×α ext ×ĥ ext and calculation of the firing values of the ith rule (i = 1, ..., 27). Let χ j denote the jth dimension of the observation vector (j = 1, 2, 3), then the firing value γ i j represents the fitting degree of the observation χ j to the antecedent fuzzy set X i j in the jth dimension of the ith rule as where X * j χ j is the fuzzified observation, moreover, b ij and c ij denote the mean and standard deviation of the Gaussian function antecedent defined in the j th dimension of the i th rule. 2. Calculation of the applicability measure of the i th rule, denoted by γ i , as the minimum of the aforementioned firing values. This weight determines the significance of the consequent fuzzy set κ i defined in the i th rule.
3. Computation of the crisp output K as the weighted average over all rule outputs: The proposed fuzzy inference machine is a zero-order Sugeno system. The complete inference for the adaptive measurement noise covariances in each epoch k can be given in a compact form as where γ i (f ), γ i (α ext ), and γ i (ĥ ext ) are the i th -rule fired membership function values and κ i denotes the singleton value of the consequent weighting factor of the i th rule for scaling the noise covariance R (see Figures 2 and 3).

Experimental Validation
This section describes the test platform employed in the evaluation of filter performance, the optimization approach utilized to tune the filter parameters, and the attitude determination results during different dynamic motions and external perturbations.

Test Environment
A comprehensive framework was designed, in which a 6 DOF test bench dynamically altered the pose (position and orientation) of a MARG unit. The 6 DOF test bench was utilized to both simulate various (accelerating, non-accelerating, and vibrating) dynamic behaviors and measure the real attitude of the sensor frame, along with the raw MARG data. The framework was based on the widely used ROS and the Gazebo open source dynamics simulator, which utilizes physics engines to consider the effects of gravity, friction, and forces [68]. As a result, this framework enabled the evaluation of state estimation error, quantification of the filter performance, and tuning of filter parameters.
The proposed test bench consisted of three prismatic joints and three revolute joints. The prismatic joints made the sensor frame slide back and forth, up and down in the three dimensional (3D) space by three 3m long rails. The revolute joints set the instantaneous attitude (Euler angles) of the sensor frame. The MARG unit is attached to a plate at the end of this kinematic chain and, so, the 6 DOF system enabled both the spatial coordinates and orientation of the sensor frame to be set and measured. Moreover, this 6 DOF mechanism enabled the generation of external accelerations simultaneously with sensor frame oscillations. Therefore, a variety of dynamic (vibrating and accelerating) system conditions could be simulated, where both the raw sensor data and real joint states were be recorded. Figure 5 shows the model of the test environment in Gazebo. Let x b , y b , and z b denote the spatial coordinates of the body plate (i.e., the origin of the MARG unit). Then, the total kinetic energy T of the test platform is given as where m j = m j,1 , m j,2 , m j,3 T , m j,i denotes the mass of each prismatic joint for i = {1, 2, 3}, whereas indicate the mass and moment of inertia of the body plate, respectively.
Moreover, q = (x b , y b , z b , φ, θ, ψ) T denotes the vector of generalized coordinates. The potential energy stored in the system is approximated as P = m b + ∑ 3 i m j,i gh 0 + m b + m j,3 gz b − m j,3 gh 1 , where the constants h 0 and h 1 denote the base height and distance between the body plate and third prismatic joint, respectively. The Lagrange function of the system is L = T − P, where the motion of the system can be determined with the help of the Lagrange equations [69]. As a result, the equations of motion can be written in the following form, where M (q) is the inertia matrix, V (q,q), including the Coriolis, centrifugal, and potential force terms, whereas τ a and τ f indicate the generalized external torques and friction effects, respectively. The aforementioned dynamics were implemented in an Unified Robot Description Format (URDF) file [70]. This file enables the specification of the whole geometric description of the system, including the robot kinematics, motion ranges, location of frames, mass properties, and collisions. Each joint (DOF) is driven in a closed-loop with an independent proportional-integral-derivative (PID) effort controller. Each effort controller was implemented, using the ros controllers meta-package, as a single-input single-output (SISO) low-level controller, in which torque control action was applied to the joint. The PID parameters were set up heuristically by iterative tuning in Gazebo. The true linear and angular positions of each joint were supplied by the joint state controller, a sensor controller that publishes the joint state information (i.e., true positions, velocities, and efforts are represented in double-precision floating-point format without measurement noise, discrepancy, or delay) [71,72]. In this application, the joint state information was obtained with a f s = 1 kHz sampling frequency. The sensor measurements were provided by independent Gazebo plugins, developed in [73]. These IMU and magnetic field sensor plugins were attached to the body plate of the 6 DOF test bench by including them in the URDF file.
To execute different acceleration and vibration dynamic motions, on one hand, random desired values were generated with random frequencies for the PID controllers of the three prismatic joints in their configuration space. On the other hand, different sinusoidal signals were supplied as reference values to the PID controllers of the three revolute joints, where both the amplitude and frequency were varied randomly. Therefore, the closed-loop system caused the 6 DOF mechanism to execute a wide variety of dynamic movements in the 3D space, with continously varying oscillations and accelerations. Simultaneously, the three prismatic joints made the sensor frame slidd back and forth, as well as up and down; simulating various external accelerations. The true joint states, along with the instantaneous MARG sensor data, were collected to evaulate the attitude estimation performance.
Video demonstrations of the closed-loop dynamics have been shared on our website. Moreover, as was described in Section 1, the whole ROS package, which includes the test bench properties, URDF files, applied effort controllers, and Gazebo configuration files, have been made publicly available in the supplementary online material, to help other lab teams evaluate similar experiments [58].

Magnetic Perturbations
Magnetic perturbations were generated artificially, as the Gazebo simulation environment does not contain such a feature. Therefore, based on the experimental results with magnetic disturbances conducted in [74][75][76][77], we developed a simple algorithm to generate magnetic perturbations. The algorithm is composed of three main steps, which are described as follows.
1. Generate a perfect artificial signal m of length L m as a mixture of square, saw-tooth, triangle, and two sinusoidal signals. Both the sequence of these signals and their parameters (i.e., the amplitude and frequency) are randomly selected. 2. Obtain the analytic signal m a from m, where the real part is the original signal, while the imaginary part contains the Hilbert transform (i.e., the original signal with a π/2 phase shift [78]). Then, generate the artificial perturbation m p as the sum of the imaginary part and absolute value of the Hilbert transformed complex signal, where the sequence of absolute values is reversed in time: 3. Remove the continuous linear trend of m p and low-pass filter the detrended signal with a first order Butterworth infinite impulse response (IIR) filter.
Each step of the aforementioned algorithm is depicted in Figure 6. Moreover, Figure 7 highlights the effect of the artificial perturbation on both the norm and each component of the raw magnetometer signal. The blue curves represent the raw (calibrated, undisturbed, and normalized) magnetometer measurements and the red curves show random sections, where the magnetometer had been disturbed artificially with the proposed algorithm. These figures illustrate that the algorithm enabled both generation of realistic magnetic perturbation effects and incorporation of effects of this type of disturbance into the analysis of attitude estimation.

Tuning of Filter Parameters
Tuning of the filter parameters was executed in MATLAB on a training data set collected in the aforementioned test environment. The heuristic particle swarm optimization (PSO) algorithm was utilized for the filter tuning problem, as it does not require gradient information, guides the search well even in nonlinear noisy systems, and has demonstrated greater effectiveness and robustness than other optimization methods [79][80][81]. Both the algorithm and applied PSO-based optimization procedure have been presented in detail in our earlier works [1,82,83]; therefore, only key information is described in the following paragraphs.
The inputs of the optimization problem are the real angular positions (i.e., the true Euler angles φ k , θ k , and ψ k provided by the 6 DOF test bench) and MARG sensor data (i.e., the acceleration, angular velocity, and magnetic field measurements), whereas its outputs are the estimation errors e φ,k = φ k −φ k , e θ,k = θ k −θ k , and e ψ,k = ψ k −ψ k , whereφ k ,θ k , andφ k denote the estimated Euler angles (i.e., the outputs of the implemented filter algorithm). The PSO is a population-based search algorithm that guides the search in the search space by employing a fitness function. In our work, a complex fitness function was formulated for the problem to both quantify the differences between the true and estimated Euler angles and measure the overall filter performance. Three mean squared errors (MSE) were combined to evaluate the filtration quality. The PSO-based minimization of the following fitness function enabled the filter parameters to be successfully tuned: where k sce denotes the number of scenarios taken into account in the optimization problem; N j is the measurement length in the k th sce scenario; and e φ,k , e θ,k , and e ψ,k indicate the roll, pitch, and yaw estimation errors, respectively. The optimization algorithm determined the optimal possible filter parameters, corresponding to the lowest possible fitness function value. The block diagram of the filter parameter optimization procedure is depicted in Figure 5. The optimization could begin running once the parameters were initialized. The PSO parameters were selected based on earlier studies [84,85], whereas the filter parameters (x 0 , P 0 , Q, and R) were initialized by employing the results presented in [1]. As the sampling time in the ROS-based framework was relatively low (T s = 1ms), the adaptive strategy could be executed with bigger window size of L = 400; moreover, the length of the transform was L FFT = 2 9 and the threshold oscillation frequency and amplitude were f thr = 10 Hz and |Ω| thr = 0.26 rad/s, respectively. The process noises µ q k and ν k in Equation (16) were considered to be statistically independent [1,19,41]; therefore, diagonal matrices were applied for both the process and measurement noise covariances with the following characteristics, where the Q q , Qω, and ρ constant noise variances were tuned with PSO. Namely, the optimization converged the quaternion measurement noise variance to a higher value of ρ = 3.53. This outcome was expected, as intense accelerations and vibrations were applied with the 6 DOF test bench and the effects of these external distrubances were absorbed in the measurement noise v k in Equation (17). This high-noise variance value indicates that the TRIAD-based attitude realization was significantly more unreliable than the gyro-based state propagation, especially in highly dynamic states of the system. At the same time, the process noise variances converged to noticeably small values (i.e., Q q = 1.45 × 10 −6 and Qω = 9.71 × 10 −10 ), resulting in the state-space dynamics (Equation (16)) becoming much more reliable than the measurement correction equations. The successful optimization contributed to finding the tuned EKF parameters which provided satisfactory attitude estimation quality with the help of the adaptive strategy described in Section 4 for both static and extreme (vibrating and accelerating) dynamic conditions.

Results
The attitude estimation performance of the FAEKF was evaluated on three measurements performed in the test environment (Measurements 1-3 lasted for approximately 160 s, 210 s, and 315 s, respectively). The dynamic motions executed by the 6 DOF test bench during these measurements included stationary states, slow and fast changes in angular positions, mild and intense oscillations, and external accelerations. The dynamic circumstances in which the filter performance was investigated were characterized by the following ranges; 0 − 8 Hz for sensor frame oscillation frequency, ±50 rad/s for angular velocity, ±16 g for external spatial acceleration, and 0 − 5 nu for magnetic perturbation magnitude.
The robust filter performance in the highly disturbed (accelerating and vibrating) test environment is highlighted in Figures 8-11. The first three rows of each figure show the roll (φ), pitch (θ), and yaw (ψ) angles, where the blue curve indicates the true Euler angles (obtained by the joint states ROS topic), whereas the red and yellow curves highlight the attitude estimation with and without the proposed fuzzy adaptive strategy, respectively. The fourth rows depict both the instantaneous external acceleration (blue curves) executed by the 6 DOF test bench and average dynamic acceleration (red curves) determined by Equation (26). Similarly, the blue curve of the fifth row of each figure shows the instantaneous magnetic perturbation generated by Equation (34), whereas the red curve indicates the average magnetic field difference calculated by Equation (27). Finally, the sixth row depicts both the instantaneous angular rate magnitude (blue curves) and the oscillation frequency of the sensor frame (red curves) determined by Equation (25). The last three rows illustrate that the employed measurement methods in the adaptive strategy provided useful information related to the external acceleration, vibration, and magnetic disturbance magnitudes.
The noticable performance improvement provided by the fuzzy-adaptive strategy is highlighted both by the figures and the results included in Table 2. The curves corresponding to the FAEKF output fit to the true Euler angles to a satisfactory degree, both in frequencies and amplitudes; even when extreme external perturbations were present. The effects of these disturbances drastically decreased the performance of the standard EKF. For example, Figure 8 highlights that, at approximately 50 s, an increased external acceleration and magnetic perturbation influenced the attitude estimation over a 15 s long period. During this period, the effects of these disturbances were effectively suppressed by the FAEKF; the adaptation laws enabled it to achieve satisfactory filter accuracy and convergence. It is also shown that, without the fuzzy-adaptive strategy, an unsatisfactory EKF performance was provided (Euler angles indicated with yellow curves). A similar outcome can be observed in Figure 9; namely, the external acceleration and magnetic perturbation effects in the high vibrating environment contributed to a significant decrease in the EKF estimation quality (e.g., see the yellow curves at~140 s). However, it is also shown that the adaptation laws enabled cancellation of these effects, even under diverse dynamic conditions. Under static conditions, both EKF and FAEKF provided approximately the same performance levels; these results are highlighted in Figures 10 and 11. As low-frequency oscillations along with no magnetic perturbation nor external acceleration enabled the accelerometerand magnetometer-based attitude realization (TRIAD output) to be characterized with high accuracy, the EKF could therefore provide satisfactory estimation quality based on the implemented state-space model. In these static cases, the adaptive strategy does not modify the noise variances, as the well-chosen ratio between the covariance parameters yields a satisfactory estimation quality.
The filter performance was quantified with the mean squared error (MSE) and standard deviation (STD) of the attitude estimation error. These results were calculated for each measurement (M1, M2, and M3) and are summarized in Table 2. Based on the results, a significant improvement in the overall filter performance can be observed. In each measurement case, the yaw angle estimation was characterized by the smallest errors, while slightly less robust outputs were provided for the roll and pitch angle estimation. This outcome was expected in our configuration and is related to the TRIAD algorithm's characteristics. Namely, the impact of magnetometer readings relative to the vertical axis is eliminated inŝ 2 andr 2 (see Equations (11) and (12)), therefore the pitch and roll angles were determined based on only the accelerometer measurements [23]. As the accelerometer measurements were disturbed much more heavily (via both the measurement noise and frequent external accelerations) than the magnetometer readings, therefore the disturbances influenced slightly more the roll and pitch estimation performance of the filter. Based on both the figures and Table 2, it can be concluded that a superior estimation convergence was achieved with the introduced adaptive strategy, thereby validating the performance of the proposed FAEKF approach. Nevertheless, some potential improvements are left open for investigation in future studies: 1. Employing a more robust deterministic approach to determine the quaternion from accelerometer and magnetometer observations in the measurement update state of the EKF. 2. Partitioning the fuzzy inputs and outputs into additional fuzzy sets, thereby implementing a more advanced fuzzy inference system. 3. Tuning the shapes of the applied fuzzy sets, the ranges of input and output variables, and the weights of the IF-THEN rules with the aid of optimization. 4. Varying the window size in the determination of external disturbance magnitudes, thereby providing more accurate measures for the adaptation laws. 5. Extending the state space model with external acceleration and magnetic perturbation models, where the driving Gaussian variables vary according to the external disturbance magnitudes. 6. Applying an additional output in the fuzzy inference system which weights the process noise covariance matrix.  Throughout the results, it was demonstrated that the developed methods in the adaptive strategy provided relevant information of the environment in which attitude estimation was performed. The obtained external disturbance magnitudes enabled us to form an inference mechanism that effectively manipulated the noise variances on-the-fly, thereby providing superior filter performance. As external accelerations, magnetic perturbations, and vibration are common disturbance sources in motorized mechatronic systems, the proposed method can be advantegously applied in such mechatronic systems. The paper also demonstrated the benefits of fuzzy logic, as it provided an expert-oriented approach to implementing complex relations with the help of simple heuristic IF-THEN rules. The proposed adaptation laws can be universally applied for the online tuning of any filter structure. Moreover, both the measurement methods and fuzzy inference mechanism can be intelligently employed in adaptive control solutions for mechatronic systems performing motions in unknown and/or disturbed environments (e.g., wheeled/legged robots moving on uneven terrain or UAVs maneuvering in windy environments). Table 2. Mean squared error (MSE) and standard deviation (STD) results of the investigated filters.  Figure 11. Fourth time slot from the measurements.

Conclusions
This paper proposed a novel qAEKF structure, FAEKF, in which both new measurement techniques were developed for the calculation of external disturbance magnitudes and novel adaptation laws were implemented with fuzzy logic. The EKF core incorporated a 7-dimensional state-space model for the estimation of both the quaternion and gyroscope bias vector. Three external disturbance measurement methods were fused to characterize the dynamic and/or perturbed environment in which attitude estimation was being performed. Namely, the external acceleration and magnetic disturbance magnitudes were represented with accumulated measures, which provided broad information of the instantaneous system circumstances. Moreover, the instantaneous oscillation frequency of the sensor frame was obtained as a measure of vibration magnitude. A sophisticated fuzzy inference machine was designed, which formed the relationship between these external disturbance magnitudes and the EKF parameters. The implemented fuzzy system incorporated simple heuristic IF-THEN rules, in order to consistently modify the noise variance values of the filter, thus providing accurate and robust attitude results. A novel test environment was designed for experimental validation of the proposed techniques, in which a 6 DOF test bench both executed various dynamic motions and measured the true Euler angles along with the raw MARG data. The tuning of EKF parameters was performed with the aid of the PSO algorithm. The experimental results showed that the proposed adaptive structure effectively suppresses the effects of external disturbances, thereby enabling the FAEKF to provide reliable attitude estimation results, even in extreme dynamic and/or perturbed situations. The proposed adaptive (dynamic-dependent) feature makes the FAEKF a suitable candidate for attitude estimation in mechatronic systems operating in variable conditions. Future work will involve the performance evaluation of the filter during the control of our Szabad(ka)-II hexapod robot in a disturbed environment.
Author Contributions: The proposed methodologies were developed, implemented, and experimentally verified by Á.O.; furthermore, he wrote this manuscript. I.K. and P.S. helped with planning and conducting the measurements. Z.V. and A.T. helped in analyzing the results. P.O. supervised the entire work. All authors have read and agreed to the published version of the manuscript.