Novel MARG-Sensor Orientation Estimation Algorithm Using Fast Kalman Filter

Guo, Siwen; Wu, Jin; Wang, Zuocai; Qian, Jide

doi:https://doi.org/10.1155/2017/8542153

Journal of Sensors

On this page

Abstract Introduction Results Conclusion Appendix Conflicts of Interest Acknowledgments References Copyright Related Articles

Research Article | Open Access

Volume 2017 | Article ID 8542153 | https://doi.org/10.1155/2017/8542153

Novel MARG-Sensor Orientation Estimation Algorithm Using Fast Kalman Filter

Siwen Guo,¹Jin Wu,²Zuocai Wang,³and Jide Qian³

Academic Editor: Calogero M. Oddo

Received08 Feb 2017

Accepted16 May 2017

Published24 Sept 2017

Abstract

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 proposed method with 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.

1. 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.

2. Novel Quaternion Determination Method from Accelerometer and Magnetometer

The attitude determination can be modeled with the following system:where is the DCM. and are sensor outputs from accelerometer and magnetometer in the body frame while and are reference vectors of the two sensors in the reference frame , respectively. can be represented by quaternions and it can be decomposed in columns by [10] where is the unit attitude quaternion. In our previous literature [10], it is proved thatwhere denotes the Moore-Penrose pseudoinverse. Now based on these, we introduce a novel attitude determination algorithm from accelerometer and magnetometer.

The magnetometer equation can be rewritten asWe also haveAn interesting result is that which leads to This generatesHence it is obtained thatExpanding (9), we have which can be simplified asin which 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 . Here, denotes the time epoch. Then we have where 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.

Initialize:

Initialization flag = false
Initialization accuracy:
while no abort command received do
()
() Input sensor observations:
() Normalization:
() If Initialization flag = false

Do
(a)
(b)
(c)
While


Else

()
end while

3. 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 bywhere in which denotes the angular rate from gyroscope. The discretized system is then given byAs there are stochastic noises inside the sensor outputs, both the process and observation models contain noise items; hence the system should be rewritten aswhere is the time span between time epoch and . The process noise’s variance is given bywhere 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 usingThe 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 byin whichwhose details are given in the Appendix 5. is given bywhere 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: .(2)Covariance prediction: .(3)Kalman gain: .(4)State correction: .(5)Covariance update: . 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 real-time 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.

4. 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 3-axis 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).

4.1. 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 , , and . 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 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 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.

4.2. 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 . 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 time consumption of one certain algorithm is another critical character. The comparison of time consumption of these algorithms is tested and plotted in Figure 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.

4.3. 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.

4.4. 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 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 vision-inertial 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 describe the characteristics of the estimation uncertainties, which helps the quality monitoring in a degree.

5. 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.

Appendix

Derivation of Measurement Quaternion’s Variance

Lettingwe can expand it to (A.2). Then (27) is computed by (A.3).

Data Access

The source code of the proposed algorithm is uploaded on https://github.com/zarathustr/FKF.

Conflicts of Interest

The authors declare that there are no conflicts of interest regarding the publication and related funding of this paper.

Acknowledgments

This research is supported by the National High Technology Research and Development Program of China (863 Program) no. 2015AA015408. The authors also thank Professor Hassen Fourati for providing them with the source code of LMA-CO.

References

H. Gui, G. Vukovich, and S. Xu, “Attitude stabilization of a spacecraft with two parallel control moment gyroscopes,” Journal of Guidance, Control, and Dynamics, vol. 39, no. 3, pp. 1–8, 2015.
View at: Publisher Site | Google Scholar
F. L. Markley, “Attitude determination using vector observations - a fast optimal matrix algorithm,” The Journal of the Astronautical Sciences, vol. 41, no. 2, pp. 261–280, 1993.
View at: Google Scholar
D. E. Zlotnik and J. R. Forbes, “Nonlinear estimator design on the special orthogonal group using vector measurements directly,” Institute of Electrical and Electronics Engineers. Transactions on Automatic Control, vol. 62, no. 1, pp. 149–160, 2017.
View at: Publisher Site | Google Scholar | MathSciNet
J. Wu, Z. Zhou, R. Li, and L. Yang, “Attitude determination using a single sensor observation: analytic quaternion solutions and property discussion,” IET Science, Measurement and Technology, 2017.
View at: Publisher Site | Google Scholar
J. Collin, “MEMS IMU carouseling for ground vehicles,” IEEE Transactions on Vehicular Technology, vol. 64, no. 6, pp. 2242–2251, 2015.
View at: Publisher Site | Google Scholar
S. O. H. Madgwick, A. J. L. Harrison, and R. Vaidyanathan, “Estimation of IMU and MARG orientation using a gradient descent algorithm,” in Proceedings of the IEEE International Conference on Rehabilitation Robotics (ICORR '11), pp. 1–7, July 2011.
View at: Publisher Site | Google Scholar
D. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology, vol. 17, IET, 2nd edition, 2004.
J. Wu, Z. Zhou, B. Gao, Y. Cheng, R. Li, and H. Fourati, “Fast linear attitude estimator using vector observations,” IEEE Transactions on Automation Science and Engineering, 2017.
View at: Publisher Site | Google Scholar
G. Wahba, A Least Squares Estimate of Satellite Attitude, 1965.
View at: Publisher Site
J. Wu, Z. Zhou, J. Chen, H. Fourati, and R. Li, “Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors,” IEEE Sensors Journal, vol. 16, no. 18, pp. 6997–7007, 2016.
View at: Publisher Site | Google Scholar
J. F. Vasconcelos, B. Cardeira, C. Silvestre, P. Oliveira, and P. Batista, “Discrete-time complementary filters for attitude and position estimation: Design, analysis and experimental validation,” IEEE Transactions on Control Systems Technology, vol. 19, no. 1, pp. 181–198, 2011.
View at: Publisher Site | Google Scholar
X. Yun, E. R. Bachmann, and R. B. McGhee, “A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements,” IEEE Transactions on Instrumentation and Measurement, vol. 57, no. 3, pp. 638–650, 2008.
View at: Publisher Site | Google Scholar
E. Foxlin, “Inertial head-tracker sensor fusion by a complementary separate-bias kalman filter,” in Proceedings of the IEEE 1996 Virtual Reality Annual International Symposium, pp. 185–194, 1996.
View at: Google Scholar
W. Li and J. Wang, “Magnetic sensors for navigation applications: An overview,” Journal of Navigation, vol. 67, no. 2, pp. 263–275, 2014.
View at: Publisher Site | Google Scholar
J. Marins, X. Yun, E. Bachmann, R. McGhee, and M. Zyda, “An extended Kalman filter for quaternion-based orientation estimation using MARG sensors,” in Proceedings of the RSJ/IEEE International Conference on Intelligent Robots and Systems, vol. 4, 2001, Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).
View at: Publisher Site | Google Scholar
Y. Tian, H. Wei, and J. Tan, “An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments,” IEEE Transactions on Neural Systems and Rehabilitation Engineering, vol. 21, no. 2, pp. 254–264, 2013.
View at: Publisher Site | Google Scholar
H. Fourati, N. Manamanni, L. Afilal, and Y. Handrich, “Complementary observer for body segments motion capturing by inertial and magnetic sensors,” IEEE/ASME Transactions on Mechatronics, vol. 19, no. 1, pp. 149–157, 2014.
View at: Publisher Site | Google Scholar
R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear complementary filters on the special orthogonal group,” Institute of Electrical and Electronics Engineers. Transactions on Automatic Control, vol. 53, no. 5, pp. 1203–1218, 2008.
View at: Publisher Site | Google Scholar | MathSciNet
P. Marantos, Y. Koveos, and K. J. Kyriakopoulos, “UAV State Estimation Using Adaptive Complementary Filters,” IEEE Transactions on Control Systems Technology, vol. 24, no. 4, pp. 1214–1226, 2016.
View at: Publisher Site | Google Scholar
R. E. Kalman, “A new approach to linear filtering and prediction problems,” Journal of Fluids Engineering, vol. 82, no. 1, pp. 35–45, 1960.
View at: Publisher Site | Google Scholar
T. Ainscough, R. Zanetti, J. Christian, and P. D. Spanos, “Q-method extended kalman filter,” Journal of Guidance, Control, and Dynamics, vol. 38, no. 4, pp. 752–760, 2015.
View at: Publisher Site | Google Scholar
S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004.
View at: Publisher Site | Google Scholar
A. M. Sabatini, “Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing,” IEEE Transactions on Biomedical Engineering, vol. 53, no. 7, pp. 1346–1356, 2006.
View at: Google Scholar
J. L. Crassidis and F. L. Markley, “Unscented filtering for spacecraft attitude estimation,” Journal of Guidance, Control, and Dynamics, vol. 26, no. 4, pp. 536–542, 2003.
View at: Publisher Site | Google Scholar
R. G. Valenti, I. Dryanovski, and J. Xiao, “A linear Kalman filter for MARG orientation estimation using the algebraic quaternion algorithm,” IEEE Transactions on Instrumentation and Measurement, vol. 65, no. 2, pp. 467–481, 2016.
View at: Publisher Site | Google Scholar
Z. Q. Zhang, “Two-step calibration methods for miniature inertial and magnetic sensor units,” IEEE Transactions on Industrial Electronics, vol. 62, no. 6, p. 1, 2015.
View at: Publisher Site | Google Scholar
Z. Zhou, Y. Li, J. Zhang, and C. Rizos, “Integrated Navigation System for a Low-Cost Quadrotor Aerial Vehicle in the Presence of Rotor Influences,” Journal of Surveying Engineering, vol. 4, no. 1, pp. 1–13, 2016.
View at: Google Scholar

Copyright

Copyright © 2017 Siwen Guo et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

PDF Download Citation

Download other formats

Order printed copies

Views

6143

Downloads

2528

Citations