Journal of Sensors

Volume 2018, Article ID 8787236, 11 pages

https://doi.org/10.1155/2018/8787236

## Simplified Attitude Determination Algorithm Using Accelerometer and Magnetometer with Extremely Low Execution Time

^{1}State Key Laboratory of Networking and Switching Technology, Beijing University of Posts and Telecommunications, Beijing 100876, China^{2}Beijing University of Posts and Telecommunications, Beijing 100876, China^{3}School of Automation, University of Electronic Science and Technology of China, Chengdu, China

Correspondence should be addressed to Xiangyang Gong; nc.ude.tpub@gnogyx and Jin Wu; moc.liamtoh@ctseu_uw_nij

Received 18 September 2017; Accepted 6 September 2018; Published 5 November 2018

Academic Editor: Francesco Dell'Olio

Copyright © 2018 Zhuohua Liu 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

Accelerometer-magnetometer attitude determination is a common and vital medium processing technique in industrial robotics and consumer electronics. In this paper, we report a novel analytic attitude solution to the accelerometer-magnetometer combination in the sense of Wahba’s problem. The Davenport matrix is analytically given and its eigenvalues are computed. Through derivations, the eigenvalues are simplified to very short expressions. Then, the corresponding eigenvectors are given accordingly via matrix row operations. The system is highly optimized based on the factorization and simplification of the obtained row-echelon form, which makes it computationally fast in practice. In this way, it is named as the fast accelerometer-magnetometer combination (FAMC). Experiments on the correctness and advantages of the proposed solution are conducted. The results show that compared with conventional solutions, the proposed analytic solution is not only correct and accurate, but to our knowledge, the most time efficient as well.

#### 1. Introduction

With the development of microelectromechanical system (MEMS) technology, tiny orientation sensors have become a mandatory component in modern consumer electronics including smartphones, wearables, and drones [1–4]. The most typical integrated sensor among others is the accelerometer-magnetometer combination (AMC) [5]. According to a basic search on the production list of the main electronics suppliers, there are over 6 types of such sensor, for example, Invensense MPU-925x, NXP/Freescale FXOS8700CQ6, Bosch BMX055 and BNO055, STMicroelectronics LSM303D, and Kionix KMX61G [6–10]. The AMC integrates the local gravity and the earth’s magnetic field together, forming a full-attitude estimation system [11]. Such a module is always designed to be power saving, compared with relatively power-consuming devices like gyroscopes. The concept of a virtual gyro involves extracting the angular rate from an AMC that later would be used for motion sensing, gesture detection, and so on [12]. To be consistent with such a scheme, it is necessary to discover one accurate and time-efficient algorithm for attitude estimation from the AMC [13].

In the past decade, there have been several approaches dealing with the AMC attitude determination problem. The early research is based on the electronic compass where the Euler angles are computed using the trigonometric relationships between sensor axes [14]. According to the singularity of Euler angles in the presence of gimbal lock, such algorithm is replaced by the optimization from a quadratic nonlinear system of quaternions. The Gauss-Newton algorithm (GNA) is applied to obtain the solution of the system [15]. Other optimization techniques, including the improved GNA (IGNA, [16]), gradient descent algorithm (GDA, [17]), and Levenberg-Marquardt algorithm (LMA, [18]), are then investigated for this problem. The above methods require recursive computation until the convergence of the solution. Therefore, they are usually accelerated by adding the gyroscope for predicting attitude kinematics.

Apart from recursive methods, batch-processing algorithms are very popular in engineering practice as well. The most well-known attitude determination formulation should be Wahba’s problem [19], proposed in 1965 providing the least-square loss function between sensor vector observations and their relative weights. Wahba’s problem has been effectively solved via quaternion estimator (QUEST, [20]), fast optimal attitude matrix (FOAM, [21]), the estimator of quaternion (ESOQ, [22]), singular value decomposition (SVD, [23]), Riemannian-manifold method [24], our recent fast linear attitude estimator (FLAE, [25]), and so on. These algorithms jointly solve Wahba’s problem by computing the maximum eigenvalue of the Davenport matrix that is first proposed in [26]. Wahba’s solutions are fast but there is a specific problem for an AMC where the magnetometer’s reference vector should be measured before attitude determination. Such reference information varies, in fact, with the position of the operated device which limits its use when there is totally no prior knowledge of global position status [5, 27, 28]. A typical commitment for an accurate heading reference system is to approximate the local magnetic field via GPS positions. However, when the magnetic field is distorted by outer disturbances, such method would have big biases not only on yaw, but on roll and pitch as well.

The core solution to the above problem is to dynamically compute the magnetic reference vector. For recursive methods, they usually obtain the information by rotating the current magnetic measurement via the last estimated quaternion so that the roll and pitch angles can hardly be interfered. Some other useful methods are developed using the geometric relationships inside the sensor combination. For instance, the factored quaternion algorithm (FQA, [29]) is designed by Yun et al. in the way of rotating each Euler axis via elementary quaternions. Based on the AMC itself, this algorithm does not require a magnetometer reference vector. According to the absence of trigonometric functions, singularity is actually presented and solved by the singularity avoidance rotation. It has been verified as an accurate method for human motion tracking and vehicle attitude determination. Besides, recently an algebraic QUAternion algorithm (AQUA, [30]) is proposed by Valenti et al. for AMC attitude determination as well. The algorithm is based on the independent limiting quaternion solutions from an accelerometer and magnetometer, respectively. It has been proved effective and does not rely on the external information of a magnetometer reference vector, too.

The aforementioned electronic design of an AMC requires a fast and accurate attitude determination algorithm. In fact, facing the two-vector case, Markley developed very fast general closed-form solutions to Wahba’s problem in [31, 32]. Also, recently with the publication of our new algorithm FLAE, the conventional Wahba problem can be solved with much faster computation speed. It seems that the research of this topic does not have the necessity to continue. However, the previous works mainly concern the sensor fusion logics, rather than the internal computations. For a platform with low hardware configuration, it is required to optimize all the architectures so that the system may execute in good status. What has not been considered by former researchers in related works is the internal sensor fusion matrix operations inside the attitude determination. For the specific case of the AMC, invoking the above tools may give further simplified solutions. Based on such purpose, we simplify the math internal of the AMC-based Wahba problem and produce some very useful analytic results, with which the accuracy is maintained while the time consumption is significantly reduced, giving the framework of the fast accelerometer-magnetometer combination (FAMC). The main contributions are summarized as follows: (1)The AMC fusion equation is reviewed and the AMC Wahba problem is revisited. Analytic eigenvalue results are given for the dynamic magnetometer reference vector case(2)The quaternion solution is given through significantly simplifying the original Wahba solution from the Davenport -method. Mandatory parameters are presented for fast computation.(3)The proposed FAMC is validated using real-world experiments. It is compared with representative methods mentioned above to give its advantages especially on time consumption.

This paper is briefly structured as follows: Section 2 contains a brief review of Wahba’s problem and presents the analytic results and related derivations. Section 3 involves the experimental validation and comparisons with representative methods. Section 4 consists of concluding remarks.

#### 2. Proposed Analytic Solution to AMC

The AMC can be related by the direction cosine matrix (DCM) **C** [33], such that
where and are normalized observation vectors from the accelerometer and magnetometer in the body frame , respectively. In this paper, the reference frame *r* is chosen as the North-East-Down (NED) coordinate system. Hence, the reference vectors here are given by and [34]. Note that the magnetometer’s reference vector is actually related to the local magnetic dip angle by [35].

Since the dip angle has a range of we obtain . Wahba’s problem gives a deterministic solution to (1) by relating the two vector observation pairs with weights. Here, defining the two weights as 0.5 equally, we have the following Wahba loss function:

Minimizing the above loss function is equivalent to calculating the maximum eigenvalue of the Davenport matrix [24]: where denotes the vector-fronted attitude quaternion while is the eigenvalue of . is given by in which where denotes the trace of a squared matrix and stands for the element of in the th row and th column.

We first obtain the symbolic eigenvalues of and the results can be given by where the parameters are

The two parameters can be simplified by invoking as

An implicit constraint inside (1) is given by which can be expanded to

This shows that the AMC fusion equation is actually self-constrained and does not require the outer information of the magnetic dip angle. Inserting (13) into (8) and (11), the four eigenvalues of are computed by

*Remark 1 . *Through the above derivations, we find out that the attitude quaternion is the eigenvector of corresponding to the maximum eigenvalue 1. Equation (14) also indicates that when the AMC is operated near the north and south poles, as approaches 1, there may be two ambiguous quaternion solutions corresponding to the eigenvalue 1. At this time, the solution is chaotic since the system cannot distinguish which one is the best quaternion. Thus, the AMC is useless in polar regions.

Then, our task is to compute the eigenvector of the eigenvalue 1. Let us define which can be further transformed into the row-echelon form with matrix row operations. The transforming matrices of the three main steps are defined as which enables as the row-echelon form. According to basic matrix knowledge, these matrices are given by where are three pivots, which are chosen based on standard row operations for the row-echelon form. and are transformed from that can be given by Since is formed by two vector observation pairs, its composition holds, namely the second-column elements are zeros. Now let Then can be expanded as follows [25]: where We may see that in these parameters, , , and are not used. Then, the other required parameters are listed in Algorithm 1. Finally, the unnormalized scalar-wise quaternion is given by the following fundamental solution: which can then be normalized with

*Remark 2 . *In the above derivations, the attitude quaternion is based on Wahba’s problem, that is, a classical least square. However, the aforementioned magnetometer’s reference vector is real-time computed which does not cope with the original definition of Wahba’s problem that the reference vectors are fixed. For the current problem, the original least-square problem is actually converted to a total least-square (TLS) problem where the observation and reference vectors are both corrupted by the noises. It has been proved by Chang that such total least-square Wahba’s problem is essentially equivalent to the conventional one [36]. Therefore in the above computations, the optimality of the algorithm has not been altered. The covariance of the attitude quaternion can be given by the results from QUEST, ESOQ, and so on, except that it should be prior multiplied by the conversion matrix
to ensure correct correspondence for a scalar-wise quaternion.