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

Authors: Zhuohua Liu; Wei Liu; Xiangyang Gong; Jin Wu
Journal: Journal of Sensors
 (2018)

---

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


---

## Body

## 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 Davenportq-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
(1)Ab=CAr,Mb=CMr,where Ab=ax,ay,azT and Mb=mx,my,mzT are normalized observation vectors from the accelerometer and magnetometer in the body frame b, 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 Ar=0,0,1T and Mr=mN,0,mDT [34]. Note that the magnetometer’s reference vector is actually related to the local magnetic dip angle θ by [35].
(2)mN=cosθ,mD=−sinθ.Since the dip angle has a range of(3)θ∈−π2,π2,we obtain mN∈0,1. 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:
(4)LC=1212Ab−CAr2+12Mb−CMr2.Minimizing the above loss function is equivalent to calculating the maximum eigenvalue of the Davenport matrixK [24]:
(5)Kq=λmaxq,where q denotes the vector-fronted attitude quaternion while λ is the eigenvalue of K. K is given by
(6)K=B+BT−trBIzzTtrB,in which
(7)B=12AbArT+MbMrT,zT=B23−B32,B31−B13,B12−B21,where tr· denotes the trace of a squared matrix and Bij stands for the element of B in the ith row and jth column.We first obtain the symbolic eigenvalues ofK and the results can be given by
(8)λK,1=122mNt1+t2,λK,2=12−2mNt1+t2,λK,3=12−2mNt1+t2,λK,4=122mNt1+t2,where the parameters are
(9)t1=ax2my2+ax2mz2−2axaymxmy−2axazmxmz+ay2mx2+ay2mz2−2ayazmymz+az2mx2+az2my2,t2=ax2+ay2+az2+mD2mx2+mD2my2+mD2mz2+mN2mx2+mN2my2+mN2mz2+2axmDmx+2aymDmy+2azmDmz.The two parameters can be simplified by invoking(10)ax2+ay2+az2=1,mx+my+mz=1,mN2+mD2=1,as
(11)t1=ax21−mx2+ay21−my2+az21−mz2−2axaymxmy−2axazmxmz−2ayazmymz=1−axmx+aymy+azmz2,t2=2+2mDaxmx+aymy+azmz.An implicit constraint inside (1) is given by
(12)AbTMb=CArTCMr=ArTCTCMr=ArTMr,which can be expanded to
(13)mD=AbTMb=axmx+aymy+azmz.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 K are computed by
(14)λK,1=122mN2+2+2mD2=1,λK,2=12−2mN2+2+2mD2=124mD2=mD,λK,3=12−2mN2+2+2mD2=124mD2=−mD,λK,4=122mN2+2+2mD2=−1.Remark 1.
Through the above derivations, we find out that the attitude quaternion is the eigenvector ofK corresponding to the maximum eigenvalue 1. Equation (14) also indicates that when the AMC is operated near the north and south poles, as mD 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(15)S=K−I,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 Λ1,Λ2,Λ3 which enables T=Λ3Λ2Λ1S as the row-echelon form. According to basic matrix knowledge, these matrices are given by
(16)Λ1=111−S41111−S31111−S211111α1111,Λ2=111−S~42111−S~32111−S~1211111α211,Λ3=111−S⌢43111−S⌢23111−S⌢13111111α31,where α1,α2,α3 are three pivots, which are chosen based on standard row operations for the row-echelon form. S~ and S⌢ are transformed from S that can be given by
(17)S~=Λ1S,S⌢=Λ2Λ1S.
SinceB is formed by two vector observation pairs, its composition holds, namely the second-column elements are zeros. Now let
(18)B=B110B13B210B23B230B33,(19)Λ1=Y11Y121Y131Y141,Λ2=1Y21Y22Y231Y241,Λ3=1Y311Y32Y33Y341.
ThenT can be expanded as follows [25]:
(20)T=1a1b1c0,where
(21)a=B23Y11+Y12Y21+Y23Y31+Y13Y31−B13−B31Y21+Y23Y31−Y31B21,b=B23Y12Y22+Y23Y32+Y13Y32−B13−B31Y22+Y23Y32−Y32B21,c=B23Y13Y33+Y12Y23Y33−Y33B21−Y23Y33B13−B31.We may see that in these parameters, Y14, Y24, and Y34 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:
(22)q~=−1,a,b,cT,which can then be normalized with
(23)q=1a2+b2+c2+1−1,a,b,cT.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
(24)R=1111,to ensure correct correspondence for a scalar-wise quaternion.Algorithm 1: The calculation procedure of the Fast Accelerometer-Magnetometer Combination (FAMC).
While no stop command received
(1)Input: Get vector observations from accelerometer and magnetometer: Ab,Mb. Normalize them with
Ab=AbAb,Mb=MbMb
(2)Preparation: Calculate the dynamic magnetometer reference vector by
mD=axmx+aymy+azmz
mN=1−mD2
(3)Fusion: Compute parameters via
B11=12mNmx,B13=12ax+mDmx
B21=12mNmy,B23=12ay+mDmy
B31=12mNmz,B33=12az+mDmz
τ=B13+B31
α1=B11−B33−1
α2=B212α1−B11−B33−1
α3=p1−2+τ2p1+Y232p2
Y11=−1α1,Y12=B21α1,Y23=τα1
Y21=−B21α1α2,Y22=−1α2,Y23=B23+B21Y13α2
Y31=τ+B21Y23α1α3,Y23=Y23α3,Y23=1α3
Obtain the quaternion elements with
a=B23Y11+Y12Y21+Y23Y31+Y13Y31−B13−B31Y21+Y23Y31−Y31B21
b=B23Y12Y22+Y23Y32+Y13Y32−B13−B31Y22+Y23Y32−Y32B21
c=B23Y13Y33+Y12Y23Y33−Y33B21−Y23Y33B13−B31
(4)Normalization:
q=1/a2+b2+c2+1−1,a,b,cT


## 3. Experiments and Results

In this section, we conduct several experiments to show the superiority of the proposed FAMC on various aspects with representative algorithms.


### 3.1. Accuracy

A specially designed inertial measurement unit (IMU) is utilized to collect raw sensor data for further evaluation (see Figure1). The Analog Device Inc. ADXRS642 MEMS gyroscopes and ADXL203 2-axis accelerometers are connected to the AD7689 16-bit ADC that is attached to the main hardware. This hardware is configured by an STM32F411CEU6 microcontroller unit (MCU) with a 120 MHz clock speed. The SPI-driven analog-digital converter (ADC) is attached to the MCU for data sampling from analog gyroscopes and accelerometers. A Honeywell HMC5983 3-axis magnetometer is wired to the MCU via SPI as well. With a 10-order sum filter and 16-order digital low-pass filter (LPF) having a cutoff frequency of 30 Hz, the acceleration and angular rate vectors are sampled at the rate of 1 kHz. According to the limitation of the HMC5983, the magnetometer can reach the highest sampling rate of 220 Hz. In this way, the data acquisition on the serial bus between the MCU and UART data logger is triggered at 200 Hz. All the sensor sampling operations are accomplished with interrupt service routines which ensure the real-time performance of the logged data. A MicroStrain 3DM-GX3-25 attitude and heading reference system (AHRS, see Figure 2) is also attached to the UART data logger that produces true reference Euler angles and quaternions. We use a MacBook Pro 2015 Retina with an i7-4core CPU and 1 TB SSD to carry out further evaluations on the MATLAB r2016b software. The AMC is precalibrated before experiments using the two-step algorithm developed by Zhang to cancel the biases and nonorthogonalities [37]. What has to be noted is that the joint axes between the two accelerometers are aligned before experiments.Figure 1
Specially designed IMU module for performance validation.Figure 2
The 3DM-GX3-25 is employed as an accurate reference source.The accelerometer measures the gravity accurately when there is minimal external acceleration. To maintain a relatively smooth attitude in the experiment, the designed IMU is mounted at the bottom of a stabilized camera on a minielectronic car (see Figure3).Figure 3
Electronic car for static performance evaluation.Vibration absorbers are installed between the vehicle and the DJI Ronin 3-axis stabilized gimbal, ensuring smooth measured motion on the camera. The car is operated using a 2.4 G remote control while the gimbal is operated with another remote control that changes the camera’s pose. In our experiments, the proposed FAMC is compared with four representative methods, that is, our recent FLAE for fast Wahba’s solution, Markley’s closed-form solution, Yun’s FQA, and Valenti’s AQUA. One period of the car in-run motion is logged via the equipment. The attitude determination results are computed and shown in Figure4. The attitude root mean-squared errors (RMSE) with respect to the reference Euler angles are summarized in Table 1. It can be observed by the readers that the macroscopic results well fit the reference Euler angles jointly. According to the calculated RMSEs, the attitude determination accuracies are basically similar. This shows that the proposed FAMC can produce determination results with the same attitude accuracy as those of representative approaches. In fact, when dealing with the AMC attitude problem, the algorithms are equivalent in accuracy. This is because they all integrate the sensors to the best. That is to say that based on AMC only, the accuracy has reached its limit. By optimizing the calculation time, the AMC computation can even be faster, which is shown in the following sections.Figure 4
Static attitude results from reference source and various algorithms.Table 1
Attitude RMSEs of various algorithms.
Algorithm
Roll
Pitch
Yaw
FLAE
2.322636301722257°
2.873468274936176°
8.301612945914364°
FQA
2.322636301722158°
2.873468274936186°
8.301612945914171°
Markley
2.322636301722258°
2.873468274936183°
8.301612945914380°
AQUA
2.322636301722257°
2.873468274936184°
8.301612945914380°
Proposed FAMC
2.322636301722256°
2.873468274936183°
8.301612945914371°


### 3.2. Full-Attitude Performance

To study the continuity of the estimated quaternions, our recently developed fast complementary filter (FCF, [33]) is adopted to fuse the gyrostabilized gravity and magnetic field together via various algorithms. This filter obtains the gyroaccelerometer quaternion via the following first step fusion:
(25)qacc,k=1−γΔt2Ω×+Iq^k−1+γ2Wacc+Iq^k−1,where γ denotes the complementary gain while Δt is the time span between neighboring samplings. Ω× is constituted by the following angular rate vector:
(26)Ω×=0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0,while Wacc is given by
(27)Wa=azay−ax0ay−az0ax−az0−azay0axayaz.Theq^k−1 denotes the estimated quaternion at time epoch k−1. The gravity is then computed through
(28)Gb=Cqacc,kAr.Finally, the magnetometer can be fused with gravity together, forming a full-attitude estimation. If the estimated quaternions from the accelerometer-magnetometer attitude estimator are not continuous, the final quaternion solutions would not be continuous as well. With the above compared algorithms, Figure5 depicts the attitude determination results of the full-attitude rotation sequence. We can find out that the proposed FAMC and Markley’s closed-form solution maintain good accuracies on roll, pitch, and yaw. The other three algorithms can only obtain an accurate estimation on roll and pitch but have large errors on yaw. This is because the quaternions from these algorithms are not completely continuous. The continuity problem causes the wrong results in quaternion addition. It is obvious from the results that the proposed FAMC does not have such a problem since it has good yaw fitness with reference angles. This characteristic of FAMC ensures robustness for sensor integration in the face of full-attitude estimation.Figure 5
Full-attitude determination results from reference source and various algorithms.


### 3.3. Magnetic Distortion

Using the designed IMU, we capture one period of raw data in which the magnetic field is distorted by an iron stick. The corresponding attitude determination results are summed up in Figure6. The results indicate that the five algorithms are jointly immune to the outer magnetic disturbance for roll and pitch angles, which coincides with the concluding remarks in the experimental sections in existing literature [29, 30]. The performance gives us the proof that the proposed FAMC is as robust as other algorithms encountering magnetic distortion.Figure 6
Attitude determination results in the presence of magnetic distortion.


### 3.4. Computation Time

One of the most significant specifications of the proposed FAMC is that it is faster than any other existing AMC fusion algorithm to our knowledge. Here, there is no need to compare with the optimization-based algorithms since they require several iterations to converge to certain accuracy. The former four algorithms are then compared with the proposed FAMC on time consumption. In this subsection, we use a data set with nearly 15 min of sampling time for evaluation. Each algorithm is executed 100 times and the time consumption is averaged. The results are plotted in Figure7. It is noticeable that the FAMC consumes the least time among all the methods. Compared with our recent fast algorithm FLAE, the FAMC advances the computation time for over 25%. In fact, not only the absolute computation time needs to be compared, but so does its standard deviation. The standard deviation of the execution time actually reflects the time variance which significantly determines its performance and reliability in embedded applications with harsh requirements of real-time processing.Figure 7
Time consumption of various algorithms.If the concentration ratio is too large, the scheduling of the overall system would be influenced accordingly. For the data in this subsection, the standard deviations of computation time is calculated and given in Table2. We can see that the FQA owns the worst standard deviation of computation time while FAMC remains the best, which provides the users with a reliable and stable attitude solution.Table 2
Standard deviation of computation time.
Algorithm
Time consumption
FLAE
2.426197633078771 × 10−6 s
FQA
7.206775321883742 × 10−6 s
Markley
2.426197633078771 × 10−6 s
AQUA
1.908509809359827 × 10−6 s
Proposed FAMC
1.181689286568760 × 10−6 s


## 3.1. Accuracy

A specially designed inertial measurement unit (IMU) is utilized to collect raw sensor data for further evaluation (see Figure1). The Analog Device Inc. ADXRS642 MEMS gyroscopes and ADXL203 2-axis accelerometers are connected to the AD7689 16-bit ADC that is attached to the main hardware. This hardware is configured by an STM32F411CEU6 microcontroller unit (MCU) with a 120 MHz clock speed. The SPI-driven analog-digital converter (ADC) is attached to the MCU for data sampling from analog gyroscopes and accelerometers. A Honeywell HMC5983 3-axis magnetometer is wired to the MCU via SPI as well. With a 10-order sum filter and 16-order digital low-pass filter (LPF) having a cutoff frequency of 30 Hz, the acceleration and angular rate vectors are sampled at the rate of 1 kHz. According to the limitation of the HMC5983, the magnetometer can reach the highest sampling rate of 220 Hz. In this way, the data acquisition on the serial bus between the MCU and UART data logger is triggered at 200 Hz. All the sensor sampling operations are accomplished with interrupt service routines which ensure the real-time performance of the logged data. A MicroStrain 3DM-GX3-25 attitude and heading reference system (AHRS, see Figure 2) is also attached to the UART data logger that produces true reference Euler angles and quaternions. We use a MacBook Pro 2015 Retina with an i7-4core CPU and 1 TB SSD to carry out further evaluations on the MATLAB r2016b software. The AMC is precalibrated before experiments using the two-step algorithm developed by Zhang to cancel the biases and nonorthogonalities [37]. What has to be noted is that the joint axes between the two accelerometers are aligned before experiments.Figure 1
Specially designed IMU module for performance validation.Figure 2
The 3DM-GX3-25 is employed as an accurate reference source.The accelerometer measures the gravity accurately when there is minimal external acceleration. To maintain a relatively smooth attitude in the experiment, the designed IMU is mounted at the bottom of a stabilized camera on a minielectronic car (see Figure3).Figure 3
Electronic car for static performance evaluation.Vibration absorbers are installed between the vehicle and the DJI Ronin 3-axis stabilized gimbal, ensuring smooth measured motion on the camera. The car is operated using a 2.4 G remote control while the gimbal is operated with another remote control that changes the camera’s pose. In our experiments, the proposed FAMC is compared with four representative methods, that is, our recent FLAE for fast Wahba’s solution, Markley’s closed-form solution, Yun’s FQA, and Valenti’s AQUA. One period of the car in-run motion is logged via the equipment. The attitude determination results are computed and shown in Figure4. The attitude root mean-squared errors (RMSE) with respect to the reference Euler angles are summarized in Table 1. It can be observed by the readers that the macroscopic results well fit the reference Euler angles jointly. According to the calculated RMSEs, the attitude determination accuracies are basically similar. This shows that the proposed FAMC can produce determination results with the same attitude accuracy as those of representative approaches. In fact, when dealing with the AMC attitude problem, the algorithms are equivalent in accuracy. This is because they all integrate the sensors to the best. That is to say that based on AMC only, the accuracy has reached its limit. By optimizing the calculation time, the AMC computation can even be faster, which is shown in the following sections.Figure 4
Static attitude results from reference source and various algorithms.Table 1
Attitude RMSEs of various algorithms.
Algorithm
Roll
Pitch
Yaw
FLAE
2.322636301722257°
2.873468274936176°
8.301612945914364°
FQA
2.322636301722158°
2.873468274936186°
8.301612945914171°
Markley
2.322636301722258°
2.873468274936183°
8.301612945914380°
AQUA
2.322636301722257°
2.873468274936184°
8.301612945914380°
Proposed FAMC
2.322636301722256°
2.873468274936183°
8.301612945914371°


## 3.2. Full-Attitude Performance

To study the continuity of the estimated quaternions, our recently developed fast complementary filter (FCF, [33]) is adopted to fuse the gyrostabilized gravity and magnetic field together via various algorithms. This filter obtains the gyroaccelerometer quaternion via the following first step fusion:
(25)qacc,k=1−γΔt2Ω×+Iq^k−1+γ2Wacc+Iq^k−1,where γ denotes the complementary gain while Δt is the time span between neighboring samplings. Ω× is constituted by the following angular rate vector:
(26)Ω×=0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0,while Wacc is given by
(27)Wa=azay−ax0ay−az0ax−az0−azay0axayaz.Theq^k−1 denotes the estimated quaternion at time epoch k−1. The gravity is then computed through
(28)Gb=Cqacc,kAr.Finally, the magnetometer can be fused with gravity together, forming a full-attitude estimation. If the estimated quaternions from the accelerometer-magnetometer attitude estimator are not continuous, the final quaternion solutions would not be continuous as well. With the above compared algorithms, Figure5 depicts the attitude determination results of the full-attitude rotation sequence. We can find out that the proposed FAMC and Markley’s closed-form solution maintain good accuracies on roll, pitch, and yaw. The other three algorithms can only obtain an accurate estimation on roll and pitch but have large errors on yaw. This is because the quaternions from these algorithms are not completely continuous. The continuity problem causes the wrong results in quaternion addition. It is obvious from the results that the proposed FAMC does not have such a problem since it has good yaw fitness with reference angles. This characteristic of FAMC ensures robustness for sensor integration in the face of full-attitude estimation.Figure 5
Full-attitude determination results from reference source and various algorithms.


## 3.3. Magnetic Distortion

Using the designed IMU, we capture one period of raw data in which the magnetic field is distorted by an iron stick. The corresponding attitude determination results are summed up in Figure6. The results indicate that the five algorithms are jointly immune to the outer magnetic disturbance for roll and pitch angles, which coincides with the concluding remarks in the experimental sections in existing literature [29, 30]. The performance gives us the proof that the proposed FAMC is as robust as other algorithms encountering magnetic distortion.Figure 6
Attitude determination results in the presence of magnetic distortion.


## 3.4. Computation Time

One of the most significant specifications of the proposed FAMC is that it is faster than any other existing AMC fusion algorithm to our knowledge. Here, there is no need to compare with the optimization-based algorithms since they require several iterations to converge to certain accuracy. The former four algorithms are then compared with the proposed FAMC on time consumption. In this subsection, we use a data set with nearly 15 min of sampling time for evaluation. Each algorithm is executed 100 times and the time consumption is averaged. The results are plotted in Figure7. It is noticeable that the FAMC consumes the least time among all the methods. Compared with our recent fast algorithm FLAE, the FAMC advances the computation time for over 25%. In fact, not only the absolute computation time needs to be compared, but so does its standard deviation. The standard deviation of the execution time actually reflects the time variance which significantly determines its performance and reliability in embedded applications with harsh requirements of real-time processing.Figure 7
Time consumption of various algorithms.If the concentration ratio is too large, the scheduling of the overall system would be influenced accordingly. For the data in this subsection, the standard deviations of computation time is calculated and given in Table2. We can see that the FQA owns the worst standard deviation of computation time while FAMC remains the best, which provides the users with a reliable and stable attitude solution.Table 2
Standard deviation of computation time.
Algorithm
Time consumption
FLAE
2.426197633078771 × 10−6 s
FQA
7.206775321883742 × 10−6 s
Markley
2.426197633078771 × 10−6 s
AQUA
1.908509809359827 × 10−6 s
Proposed FAMC
1.181689286568760 × 10−6 s


## 4. Conclusion

In this paper, the accelerometer-magnetometer attitude determination problem is revisited. Based on Wahba’s problem, the eigen system of the transformed formulation is investigated. With skilled simplifications, the eigenvalues of the Davenport matrixK are obtained with very simple expressions, in the case of an implicit constraint inside the sensor fusion. The constraint enables the algorithm free of a predetermined magnetometer’s reference vector as well. It also reveals that AMC may be useless when operated near polar regions. Based on the solved eigenvalues, the eigenvector, or the optimal quaternion, which corresponds to the maximum eigenvalue 1 is computed with matrix row operations. Some analytic results are shown further to simplify this step which generates an explicit and fast algorithm. The suitability of the adoption of the dynamic magnetometer’s reference vector is discussed invoking a recent literature by Chang [36]. We then carry out several experiments to clarify the correctness of the validation of the proposed FAMC. Performance evaluations on common motion, full-attitude motion, and magnetic distortion are investigated. The results show that the FAMC can produce attitude determination results with the same accuracy as representative methods. It is more robust than some other algorithms in the face of full-attitude measurement. Moreover, it maintains the magnetic-distortion immunity as other methodologies. Finally, computation times of various algorithms are tested which reveals that the proposed FAMC is not only the fastest but owns the least standard deviation of time consumption as well. It is believed that the proposed FAMC can be extensively used for low-power, precision, and reliable applications in the future.



---

*Source: 8787236-2018-11-05.xml*