Journal of Sensors

Volume 2017 (2017), Article ID 8542153, 12 pages

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

## Novel MARG-Sensor Orientation Estimation Algorithm Using Fast Kalman Filter

^{1}School of Educational Software, Guangzhou University, Guangzhou, China^{2}School of Automation, University of Electronic Science and Technology of China, Chengdu, China^{3}Chengdu Institute of Computer Applications, Chinese Academy of Sciences, Chengdu, China

Correspondence should be addressed to Jin Wu

Received 8 February 2017; Accepted 16 May 2017; Published 24 September 2017

Academic Editor: Calogero M. Oddo

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.

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