Novel MARG-Sensor Orientation Estimation Algorithm Using Fast Kalman Filter

Orientation estimation from magnetic, angular rate, and gravity (MARG) sensor array is a key problem in mechatronic-related applications. This paper proposes a new method in which a quaternion-based Kalman filter scheme is designed. The quaternion kinematic equation is employed as the process model. With our previous contributions, we establish the measurement model of attitude quaternion from accelerometer and magnetometer, which is later proved to be the fastest (computationally) one among representative attitude determination algorithms of such sensor combination. Variance analysis is later given enabling the optimal updating of the proposed filter. The algorithm is implemented on real-world hardware where experiments are carried out to reveal the advantages of the proposedmethodwith respect to conventional ones.The proposed approach is also validated on an unmanned aerial vehicle during a real flight. Results show that the proposed one is faster than any other Kalman-based ones and even faster than some complementary ones while the attitude estimation accuracy is maintained.


Introduction
In many robotic applications, the system needs to obtain the orientation parameters of some vital mechanical parts so that it can precisely control the actuators [1].As is known, the orientation parameters consist of the roll, pitch, and yaw angles, namely, the frequently used Euler angles [2].Usually, MARG sensors, that is, the gyroscope, accelerometer, and magnetometer, are employed to compute the orientation [3,4].Modern technological advances have allowed for the massive production of microelectromechanical-system (MEMS) inertial sensors, which are highly compactly integrated [5].There are many other sensor combinations, for example, the gyro-accelerometer and gyro-magnetometer ones, which have been discussed in [6].In the past decade, there has been many efforts around the sensor fusion techniques of MARG sensors.
Attitude can often be acquired from integral of angular rates from gyroscope triads or least-square problem formed by vector observations [7,8].The first approach is related to the inertial navigation system (INS) in which inertial sensors are produced with tactical-grade quality.The second one is called Wahba's problem when there are at least two vector observations [9].For consumer electronics, MEMS gyroscope faces dilemma as there are too many uncertainties inside almost every process of the angular-rate sensing [10].According to this, the angular-rate integral diverges very fast as time goes.Hence, to obtain stable and accurate attitude outputs, the gyroscope integral should be compensated using other strapdown sensors [11].For instance, using accelerometer and magnetometer, it is able to determine the local gravity vector and geomagnetic heading, respectively [12].
Early concerns about the MARG-sensor fusion are almost raised around 1990s when some researchers started to develop fusion algorithms based on complementary filters.Foxlin first used MARG sensors to monitor the attitude motion of human's head [13].One general framework of sensor fusion that combines the accelerometer and magnetometer as the electronic compass (eCompass) [14].Marins et al. then tried to compute the attitude from eCompass using the Gauss-Newton algorithm (GNA) via quaternion representation [15].This commitment was revisited by Madgwick et al. in 2011 showing that the gradient descent algorithm (GDA) could be another possible solution [6].Besides, optimization methods like improved Gauss-Newton algorithm (IGNA, [16]) and Levenberg-Marquadt algorithm (LMA, [17]) are also applied for faster and more robust solutions.The above methods use optimization algorithms to compute attitude quaternion from the eCompass system.Mahony et al. tried different way where the attitude representation on special orthogonal groups is studied [18].Using transformation of Wahba's problem, Marantos et al. designed one efficient complementary filter with singular value decomposition (SVD) as well [19].
The above ones focus on complementary filter design of attitude estimator.Complementary filters are flexible for application on common platforms but still have their drawbacks.The main concern is that the complementary gain is always empirically given for one time and would result in terrible estimates for another quite different performance.The second one is that some complementary gains degenerate to constants for the sake of convenience in engineering practice which significantly limits the estimation accuracy and convergence speed.To overcome such problem, Kalman filter is employed.The Kalman filter is an optimal estimator for real-time inputs in the sense of minimal mean-squared error (MMSE) [20].Designed in this way, it owns the best estimation accuracy statistically.However, since Kalman filter is developed especially for linear system with uncorrelated white Gaussian noises (WGN), when nonlinearities are involved, the conventional filter would be no longer useful.In order to solve such problems, variants of Kalman filter like the extended Kalman filter (EKF, [21]) and unscented Kalman filter (UKF, [22]) are proposed and extensively investigated.For instance, when representing the direction cosine matrix (DCM) with quaternion, the quadratic items lead to nonlinearities.To overcome this, Sabatini designed an EKF to obtain accurate attitude outputs from MARG sensors [23].The UKF approach is proposed by Crassidis and Markley revealing higher accuracy compared with existing filters [24].It should be noted that, in such practices, extra computations like linearization, Cholesky factorization, and so forth increase the time consumption of the attitude estimation algorithm significantly.
Therefore, we can see that it is necessary to find one method that is accurate statistically but fast even compared with complementary filters.In fact, in our previous research, the attitude determination from accelerometer is computed via linear quaternion transformations.In this paper, this motivates us to develop a novel quaternion measurement algorithm from accelerometer and magnetometer jointly.The algorithm serves as the observation model of the Kalman filter where the observation variance is systematically derived.Using the quaternion kinematic equation as the process model, the Kalman filter is established.Experiments are conducted to show the performance of the proposed filter with respect to representative methods.The results show that the filter is better for time consumption and maintains high attitude accuracy.
This paper is structured as follows: Section 2 introduces novel method for attitude determination from accelerometer and magnetometer.Section 3 consists of the design and implementation of the novel fast Kalman filter.Section 4 includes the experimental descriptions, results, and comparisons.Section 5 contains the concluding remarks.

Novel Quaternion Determination Method from Accelerometer and Magnetometer
The attitude determination can be modeled with the following system: where C is the DCM.
푇 are sensor outputs from accelerometer and magnetometer in the body frame 푏 while A 푟 = (0, 0, 1) 푇 and M 푟 = (푚 푁 , 0, 푚 퐷 ) 푇 are reference vectors of the two sensors in the reference frame 푟, respectively.C can be represented by quaternions and it can be decomposed in columns by [10] where q = (푞 0 , 푞 1 , 푞 2 , 푞 3 ) 푇 is the unit attitude quaternion.In our previous literature [10], it is proved that The magnetometer equation can be rewritten as We also have ( An interesting result is that which leads to This generates Hence it is obtained that Expanding ( 9), we have which can be simplified as in which W 푚 is given in (12).Then it computed that This shows that (11) can not be solved via direct iteration, recalling that, in our previous paper [10], we proved that the stable solution of is if and only if W 2 푡 = I 4×4 .Here, 푡 denotes the time epoch.Then we have where q 푚,푡 stands for the quaternion from magnetometer at the 푡 time epoch.
Combining the previous result of quaternion from accelerometer, that is, where with the above equation of magnetometer, we obtain Algorithm 1 for attitude determination algorithm from accelerometer and magnetometer.

Kalman Filter Design
In our design, the quaternion kinematic equation is employed as the process model.The gyroscope's bias is not concerned in the current form.The gyroscope's bias and biases of the accelerometer and magnetometer for observation model are cancelled before the filter update.Using the attitude determination algorithm proposed in the last section as the observation model, we model the system by where in which 휔 = (휔 푥 , 휔 푦 , 휔 푧 ) 푇 denotes the angular rate from gyroscope.The discretized system is then given by As there are stochastic noises inside the sensor outputs, both the process and observation models contain noise items; hence the system should be rewritten as where Δ푡 is the time span between time epoch 푡 and 푡 − 1.The process noise's variance is given by where Σ gyro stands for the variance of angular rates' covariance, which is usually given by while 휎 휔  denotes the standard deviation of 푖-axis' angular rate.Ξ 푡 is given by The measurement quaternion is computed using The observation noise is induced by accelerometer and magnetometer's noises and we can see that, according to matrix multiplication, the accelerometer vector and magnetometer vector have been mixed.In this way, there are quadratic nonlinearities inside the propagation of variance.Hence, for the observation model, the variance is given by the first-order approximation.The measurement quaternion's variance is approximated by in which whose details are given in the Appendix 5. Σ acc,mag is given by where Σ acc , Σ mag are covariances of accelerometer and magnetometer, respectively.Then we can use the following algorithm to obtain MMSE estimation of attitude via Kalman filter: (1) State prediction: (3) Kalman gain: .It can be seen that the overall calculation steps are time efficient and flexible.
3.1.Discussion of the Kalman Filter.In previous descriptions, we have noted that the Kalman filter can not be applied to systems with correlation noises.As given in (25), the quaternion noise not only relates to accelerometer and magnetometer's outputs but also relates to the covariance of the optimal quaternion computed last time.However, according to realtime applications, the measurement from accelerometer and magnetometer at time 푡 has very tiny correlation with last time's quaternion.Hence it is almost independent with predicted quaternions as well.Therefore, here it is possible to use Kalman filter for attitude estimation.

Experiments and Results
In this section, we conduct several experiments using the MicroStrain 3DM-GX3-25 attitude and heading reference system (AHRS).The hardware is shown in Figure 1.The AHRS is 9 degrees of freedom (9DOF) which contains 3axis precision gyroscope, accelerometer, and magnetometer.According to the official datasheet of the AHRS, the inside attitude estimation program is formed by EKF providing users with highly reliable attitude estimation.Besides, the specially designed data transmission protocol allows for the logging of raw sensor data.In the following experiments, the data sampling frequency is set to 500 Hz and the motion is randomly generated using human's hand.The raw inertial data and the reference attitude angles are measured and collected via the USB data wire.To visually display the attitude estimation performance, we also design one upper monitor software program showing the attitude motion from the AHRS and proposed method (see Figure 2).

Experiment 1: Comparisons with Commercial AHRS.
In this experiment, we compare the attitude estimates from the proposed filter and AHRS so that the absolute accuracy of our method can be verified.The variances of gyroscope, accelerometer, and magnetometer are computed via statistical data when the AHRS is still on the table.The parameters are Σ gyro = diag(0.0011,0.0009, 0.0013), Σ acc = diag(0.01,0.098, 0.011), and Σ mag = diag(0.014,0.012, 0.020).The recorded raw sensor data is shown in Figure 3. Using our proposed Kalman filter, the estimated quaternions are converted to Euler angles and are plotted with reference angles together in Figure 4.
We can see that the proposed filter outputs accurate attitude estimates which coincide with the reference angles very well.The MSEs of roll, pitch, and yaw angles with respect to the reference are 0.0134 ∘ , 0.0286 ∘ , and 0.335 ∘ , respectively, which show that the proposed algorithm is accurate.
To verify the observation model and its variance, the measurement quaternion and its 3휎 bounds are demonstrated in Figure 5.The standard deviation of the quaternion is computed by taking the square roots of the diagonal elements of the covariance matrix.In the presented materials, we would find out that the measurement quaternions are continuous.The measurement quaternion's standard deviation bounds fit the measurement quaternion very well.Hence, the first-order approximation of the measurement quaternion is experimentally verified to be adequate to describe the noise characteristics.
Experiment 1 shows that the proposed filter can estimate attitude angles of quite the same accuracy with commercial EKF-based AHRS.However, compared with EKF, there is no doubt that the flexible design of the proposed filter allows for the huge decrease of time consumption.This is going to be shown in the next subsection.

Experiment 2: Comparisons with Representative Complementary Filters.
Using the hardware presented in Figure 1, we acquire another sample of motion's data.The raw data is processed here with the proposed filter and representative complementary ones including the LMA-Complementary Observer (LMA-CO) by Fourati et al. [17] and Wahba's Complementary Filter (WCF) by Marantos et al. [19] The factor of LMA in LMA-CO is set to 0.01 according to related literature while its complementary gain is set to 0.01 denoting the motion's change rate.The parameters of WCF follow the original author's determination that are given by 푤 푎 = 0.9, 푤 푚 = 0.8, 푐 1 = 0.7, 푐 2 = 0.3, 푐 3푎 = 8500.0,푐 3푚 = 5500.0.And for the sake of justice, the adaptive mode of WCF is not enabled.The computed Euler angles are drawn in Figure 6.
We can see that the differences between LMA-CO and the proposed filter are quite small while that between WCF and the proposed filter seems to be relatively larger.Throughout computations of MSEs with respect to reference angles, the results are shown in Table 1.
This shows that the proposed filter is more accurate than the two representative methods.In engineering practice, the   7.
Compared with LMA-CO and WCF, we may find out that the proposed filter owns the least time consumption.This is because LMA-CO uses more complicated LMA and WCF adopts SVD for attitude estimation.The proposed filter, however, has only one computationally-complex process, that is, the inversion operation.This shows that the proposed filter is faster than these representative complementary filters.

Experiment 3: Comparisons with Representative Kalman
Filter.In this subsection, we also evaluate the results from a recent representative Kalman filter, that is, the Algebraic QUAternion Kalman Filter (AQUA-KF) by Valenti et al. [25].Related variances of sensors are the same as that of the proposed filter.Using the data sample from the last subsection, the estimation results are computed and shown in Figure 8.
We can see that the AQUA-KF seems to have some minor differences with the proposed filter.For AQUA-KF, the MSEs of roll, pitch, and yaw angles with respect to reference angles are 2.24 ∘ , 1.56 ∘ , and 3.67 ∘ .The details of the proposed filter on this data sample have been shown in Table 1.We can directly see that the proposed filter is more accurate than the AQUA-KF.
Moreover, we also evaluate the time consumption performance of the two filters.The tested time consumption results are shown in Figure 9 revealing that the AQUA-KF is evidently slower than the proposed one.This is because the AQUA-KF obtains observation model via the AQUA algorithm which needs more judgement and matrix operations.

Experiment 4: Test Results on an Unmanned Aerial Vehicle.
In this experiment, the proposed approach is validated on an unmanned aerial vehicle.The designed platform is composed of an MEMS inertial sensor array, a vision-inertial AHRS, a micro controller, USB debugger, and a transmitter (see Figure 10).Using the monitoring software we designed, it is possible to log the raw data and calculate orientation visually.The MEMS inertial sensor array contains high-end 3-axis gyroscope, accelerometer, and magnetometer.
The vision-inertial AHRS is made by YunHeng Tech.Inc., Chengdu, China, giving high-resolution and precision attitude outputs according to local vision and inertial measurements.The XBee S3B Pro transmitter is used for wireless Journal of Sensors  data transmission.The raw sensor outputs and referenced information are processed with the micro controller, an STM32F407VG board with 168 MHz clock speed, and abundant interfaces for data interaction.The sensor biases are cancelled before the flight was done according to the method by Zhang [26].During a real flight in Chengdu, Sichuan Province, China, the raw measurements of accelerometer, gyroscope, and magnetometer are collected.Meanwhile, the vision-inertial AHRS provides us with the reference angles for comparison.The whole project is implemented using C++ programming language on the IAR Embedded WorkBench  IDE.The raw inertial data during the flight test is logged via the upper monitor and is plotted in Figure 11.Using this sample, the estimated Euler angles are calculated, which are compared with the outputs from the onboard visioninertial AHRS (see Figure 12).We can see from the results that the roll and pitch angles fit the reference ones very well while the yaw does not show the same accuracy.This is because when the motors on the drone are armed, the local  magnetic field is significantly disturbed, leading to the biases of the magnetometer's measurement.Such phenomenon can actually be avoided by adopting the magnetic-field modeling presented by Zhou et al. [27].The roll and pitch's performances show that the proposed filter is able to accomplish attitude estimation tasks in real-flight tests.Besides, from Figure 13, we can also find out that the variance information can well the of the estimation uncertainties, which helps the quality monitoring in a degree.

Conclusion
Orientation estimation from MARG sensors is revisited in this paper.Using our previous research contributions, we establish one observation model for determining attitude quaternions from accelerometer and magnetometer with linear quaternion transformations.Adopting the quaternion kinematic equation as the process model, we form a novel Kalman filter with minimum state dimension.The variance analysis of the observation model is systematically derived.Since the observation procedure employs merely several simple matrix multiplications, the computational speed is quite faster.Experiments on real-world hardware are carried out showing the correctness of the measurement quaternion and observation variance.Throughout comparisons of the attitude estimation results between the proposed filter and commercial AHRS, other representative filters, we not only prove that the proposed filter is accurate, but also show that the proposed filter is faster than these representative ones  including complementary filters and other Kalman filters.
The source code has been uploaded to https://github.com/zarathustr/FKF and the audience can evaluate the algorithm by downloading it.We hope that this research would benefit related attitude estimation applications in the future.

Figure 2 :
Figure 2: Designed upper monitor for visual display of attitude estimation.

Figure 3 :
Figure 3: Raw sensor data of angular rate, acceleration, and magnetic field.

Figure 5 :
Figure 5: Measurement quaternion and its standard deviation bounds.

Figure 6 :
Figure 6: Estimated Euler angles from the proposed filter, LMA-CO, and WCF.

Figure 7 :
Figure 7: Tested time consumption of different algorithms.

Figure 8 :
Figure 8: Estimated Euler angles from the proposed filter and AQUA-KF.

Figure 9 :
Figure 9: Time consumption results of the proposed filter and AQUA-KF.

Figure 11 :
Figure 11: Raw inertial sensor data of the flight test.

Figure 12 :
Figure 12: Estimated and reference Euler angles during the flight test.

Figure 13 :
Figure 13: The variance of estimated quaternions and their 3휎 bounds.
∘time consumption of one certain algorithm is another critical character.The comparison of time consumption of these algorithms is tested and plotted in Figure