Analytic accelerometer–magnetometer attitude determination without reference information

Abstract

Attitude determination using the accelerometer–magnetometer combination is basically an applied mathematical problem. Commonly, such attitude determination tasks should be accomplished with reference information of sensors, e.g. the magnetometer’s reference vector, from calibration processes. However, this piece of important information cannot be accurately acquired or would differ with the change of body’s position. In this paper, to solve such problem, the conventional accelerometer–magnetometer combination fusion equations are transformed into a brand new set of equations, which are free of reference information. Quaternion solutions to the novel equations are obtained via analytic computation techniques. According to the unique design, the proposed attitude estimator owns very fast computation speed compared with representative methods including iterative ones and algebraic ones. Experiments and simulations are carried out to give demonstrations and comparisons, which verify the correctness and effectiveness of the proposed method. The results also show that using the proposed quaternion solution, fusion with gyroscope can be more efficiently obtained.

Keywords

Attitude determination accelerometer–magnetometer combination analytic computation microelectromechanical system inertial sensors

Introduction

Microelectromechanical system (MEMS) inertial sensors have become more and more popular with the development of unmanned aerial vehicle (UAV), mobile navigation, human motion tracking, rehabilitation engineering, and etc.^1–4 In these applications, the inertial sensors like gyroscope, accelerometer, and magnetometer are fused together in order to achieve accurate and reliable attitude estimation.⁵ Technically, gyroscope, accelerometer, and magnetometer can be integrated as an Attitude Heading Reference System (AHRS).⁶ This can be obtained by means of filtering methodologies such as Kalman filter (KF) with its variants, including extended Kalman filter (EKF) and unscented Kalman filter (UKF).^7–9 Such filtering approaches usually require the determination of attitude from strapdown vector observations, which can be acquired with strapdown sensors like accelerometer, magnetometer, solar sensor, and etc.^10,11

Accelerometer–magnetometer combination (AMC), which is also referred to as the electronic compass, has been applied to low-cost attitude estimation in the past decade. Researchers have been dedicated to finding effective ways for solving the AMC-based attitude determination problem. Conventionally, the AMC attitude determination is solved by the Wahba’s problem.¹² Wahbas’s problem’s target loss function can be given by

L (C) = \frac{1}{2} \sum_{i = 1}^{n} a_{i} {‖ b_{i} - C r_{i} ‖}^{2}

(1)

where,

b_{i}

and

r_{i}

denote the observation vectors in body frame b and reference frame r of the ith sensor respectively.

C_{i}

stands for the ith direction cosine matrix (DCM) and a_i denotes the ith weight. Many effective methods like QUaternion ESTimator (QUEST),¹³ Fast Optimal Attitude Matrix (FOAM),¹⁴ EStimator of Optimal Quaternion (ESOQ),¹⁵ Fast Linear Attitude Estimator (FLAE),¹⁶ etc.^17–19 have been developed to solve the Wahba’s Problem. Some other optimization approaches are proposed e.g. the bisection method by Turkoglu and Wahl.²⁰ Since it adopts the least squared model, it is the optimal and most robust attitude determination approach.

For the AMC, the formulation turns out to be a two-vector-observation attitude problem.²¹ As described in representative methods for solving Wahba’s problem, the computational burden is considerable since many matrix operations like eigenvalue decomposition or singular value decomposition (SVD) are involved, which significantly influences the online determination ability of related algorithms. For two-vector framework, algebraic algorithms are designed like TRIAD,²² Factored Quaternion Algorithm (FQA).⁸ TRIAD is proved to be suboptimal,²³ while FQA is suboptimal as well since the singularity avoidance requires empirical choosing of threshold. What is more, for all the above representative methods, the joint drawback is that they eagerly require the magnetometer’s reference information, which can hardly be acquired in consumer electronics applications.

In fact, the AMC fusion equation can be represented with quaternions. This generates a set of quadratic quaternionic equations with nonlinearity.²⁴ In this case, such equations are solved via iterative algorithms like gradient descent algorithm (GDA),²⁵ improved Gauss–Newton algorithm (IGNA),²⁶ Levenberg–Marquardt Algorithm (LMA).²⁷ In these algorithms, the magnetometer’s reference vector is involved in the overall iteration along with the quaternion. These algorithms can solve the problem, but their main drawback is evident that they consume too much time consumption lowering the data-processing frequency of the operating platform.

Some above endeavor in the last 15 years is mainly focused on finding optimal solutions via Wahba’s problem or iterative algorithms. Others were trying to solve the problem in a suboptimal framework. However, there is hardly any report for analytic solution to such problem although we all know that analytic algorithms are much faster than numerical ones in most cases. Hence, motivated by existing methods, we mainly make the following contributions in this paper:

We simplify the fusion equations which no longer rely on the reference vector of the magnetometer.

The analytic solutions to the fusion equations are obtained.

Experiments and simulations are presented to verify the effectiveness and correctness of the proposed method.

This paper is briefly structured as follows: Sensor modeling and fusion equations section involves the sensor modeling and fusion equations. Quaternion selector and calculation steps section presents the newly transformed equations and proposed analytic solutions. In Experiments, simulations, and results section, our experimental results are illustrated to verify the effectiveness of the proposed method. Comparisons with other representative methods are also shown in this section. Appendix 1 covers some basic knowledge of quaternions and rotations.

Sensor modeling and fusion equations

Ideal normalized reference vectors of the accelerometer and magnetometer are given as follows^25,26:

{\begin{array}{l} A^{r} = {(0, 0, 1)}^{⊤} \\ M^{r} = {(m_{N}, 0, m_{D})}^{⊤} \end{array}

(2)

where, m_N and m_D are north and vertical components of the local earth magnetic field, respectively, in North–East–Down (NED) frame. This can also be computed with local dip angle ρ, such that²⁸

\begin{array}{l} m_{N} = \cos ρ \\ m_{D} = - \sin ρ \end{array}

(3)

$A^{b}, M^{b}$ denote the normalized observation vectors of the accelerometer and magnetometer in body frame b, respectively:

{\begin{array}{l} A^{b} = {(a_{x}, a_{y}, a_{z})}^{⊤} \\ M^{b} = {(m_{x}, m_{y}, m_{z})}^{⊤} \end{array}

(4)

Note that the output of the two sensor is the mixture of real values, sensor biases and noises, such that

\begin{array}{l} A^{b} = {\hat{A}}^{b} + b_{A^{b}} + w_{A^{b}} \\ M^{b} = {\hat{M}}^{b} + b_{M^{b}} + w_{M^{b}} \end{array}

(5)

where

{\hat{A}}^{b}

is the truth of

A^{b}, b_{A^{b}}

denotes the bias of the accelerometer and

w_{A^{b}}

stands for the white Gaussian noise (WGN) with the covariance of

Σ_{w_{A^{b}}}

. Notations are similar with the model of magnetometer’s output in body frame. The sensor biases would influence the attitude estimation hence in this paper the biases are calibrated using the calibration method for three-axis field sensors.²⁹ The rotation equations of the ideal sensor outputs from reference frame to the body frame can be written as²¹

{\begin{array}{l} A^{b} = C_{r}^{b} A^{r} \\ M^{b} = C_{r}^{b} M^{r} \end{array}

(6)

Analytic solving method

Transformed AMC fusion equations

Equation (6) can be further given by

{\begin{array}{l} A^{b} = C_{r}^{b} A^{r} \\ C_{b}^{r} M^{b} = M^{r} \end{array}

(7)

Expanding equation (7) using quaternions, we have:

{\begin{array}{l} 2 (q_{1} q_{3} - q_{0} q_{2}) - a_{x} = 0 \\ 2 (q_{2} q_{3} + q_{0} q_{1}) - a_{y} = 0 \\ (1 - 2 q_{1}^{2} - 2 q_{2}^{2}) - a_{z} = 0 \\ 2 m_{x} (q_{1} q_{2} + q_{0} q_{3}) + \\ m_{y} (1 - 2 q_{1}^{2} - 2 q_{3}^{2}) + \\ 2 m_{z} (q_{2} q_{3} - q_{0} q_{1}) = 0 \end{array}

(8)

While the conventional fusion equations adopted in Tian et al.,³ Madgwick et al.,²⁵ Tian et al.,²⁶ Marins et al.,³⁰ can be given by equation (6), such that

{\begin{array}{l} 2 (q_{1} q_{3} - q_{0} q_{2}) - a_{x} = 0 \\ 2 (q_{0} q_{1} + q_{2} q_{3}) - a_{y} = 0 \\ 2 (0.5 - q_{1}^{2} - q_{2}^{2}) - a_{z} = 0 \\ 2 m_{N} (0.5 - q_{2}^{2} - q_{3}^{2}) + 2 m_{D} (q_{1} q_{3} - q_{0} q_{2}) - m_{x} = 0 \\ 2 m_{N} (q_{1} q_{2} - q_{0} q_{3}) + 2 m_{D} (q_{0} q_{1} + q_{2} q_{3}) - m_{y} = 0 \\ 2 m_{N} (q_{0} q_{2} + q_{1} q_{3}) + 2 m_{D} (0.5 - q_{1}^{2} - q_{2}^{2}) - m_{z} = 0 \end{array}

(9)

It is obvious that they require the reference vector information of the magnetometer.

Solving method

We can easily obtain the solutions to the first three equations in equation (8):

{\begin{array}{l} 2 (q_{1} q_{3} - q_{0} q_{2}) - a_{x} = 0 \\ 2 (q_{2} q_{3} + q_{0} q_{1}) - a_{y} = 0 \\ (1 - 2 q_{1}^{2} - 2 q_{2}^{2}) - a_{z} = 0 \end{array}

(10)

by substituting q₃ for other variables, we obtain

{\begin{array}{l} q_{0} = \mp \frac{\sqrt{a_{z} - a_{N o r m} - 2 q_{3}^{2}}}{\sqrt{2}} \\ \begin{array}{l} q_{1} = - \frac{a_{z} + a_{N o r m}}{\sqrt{2} (a_{x}^{2} + a_{y}^{2})} \\ \times (\sqrt{2} a_{x} q_{3} \mp a_{y} \sqrt{a_{z} - a_{N o r m} - 2 q_{3}^{2}}) \end{array} \\ \begin{array}{l} q_{2} = \pm \frac{a_{z} + a_{N o r m}}{\sqrt{2} (a_{x}^{2} + a_{y}^{2})} \\ \times (\pm \sqrt{2} a_{x} q_{3} + a_{y} \sqrt{a_{z} - a_{N o r m} - 2 q_{3}^{2}}) \end{array} \end{array}

(11)

along with

{\begin{array}{l} q_{0} = \mp \frac{\sqrt{a_{z} + a_{N o r m} - 2 q_{3}^{2}}}{\sqrt{2}} \\ \begin{array}{l} q_{1} = - \frac{a_{z} - a_{N o r m}}{\sqrt{2} (a_{x}^{2} + a_{y}^{2})} \\ \times (\sqrt{2} a_{x} q_{3} \mp a_{y} \sqrt{a_{z} + a_{N o r m} - 2 q_{3}^{2}}) \end{array} \\ \begin{array}{l} q_{2} = \pm \frac{a_{z} - a_{N o r m}}{\sqrt{2} (a_{x}^{2} + a_{y}^{2})} \\ \times (\pm \sqrt{2} a_{x} q_{3} + a_{y} \sqrt{a_{z} + a_{N o r m} - 2 q_{3}^{2}}) \end{array} \end{array}

(12)

where, a_Norm is the norm of

A^{b}

and obviously it equals to 1. As far as equation (11) is concerned, we would find that the value inside the square root, namely

a_{z} - a_{N o r m} - 2 q_{3}^{2}

is always nonpositive since

a_{z} \in [- 1, 1], a_{N o r m} = 1, q_{3}^{2} \geq 0

(13)

Hence, equation (11) is meaningless for the problem discussed in this paper for components of quaternions should always be real numbers. Inserting a_Norm = 1 into equation (12) gives

{\begin{array}{l} q_{0} = \pm \frac{\sqrt{a_{z} + 1 - 2 q_{3}^{2}}}{\sqrt{2}} \\ q_{1} = \frac{a_{x} q_{3} + a_{y} q_{0}}{a_{z} + 1} \\ q_{2} = \frac{a_{y} q_{3} - a_{x} q_{0}}{a_{z} + 1} \end{array}

(14)

Inserting the components in equation (14) into the fourth equation of equation (8), we obtain the solutions of q₃

q_{3} = \pm \frac{\sqrt{χ_{1} \pm \sqrt{χ_{2}}}}{2 \sqrt{χ_{3}}}

(15)

where

χ_{1}, χ_{2}, χ_{3}

are parameters, which have the following architecture

{\begin{array}{l} χ_{1} = (1 + a_{z}) χ_{3} \\ χ_{2} = {(1 + a_{z})}^{2} χ_{4}^{2} χ_{3} \end{array}

(16)

where

χ_{4} = [- a_{x}^{2} + a_{y}^{2} + {(1 + a_{z})}^{2}] m_{x} - 2 a_{x} (a_{y} m_{y} + m_{z} + a_{z} m_{z})

(17)

Combining (equations (16) and (15), we obtain

\begin{matrix} q_{3} = \pm \frac{\sqrt{χ_{1} \pm \sqrt{χ_{2}}}}{2 \sqrt{χ_{3}}} = \pm \frac{1}{2} \sqrt{\frac{χ_{1} \pm \sqrt{χ_{2}}}{χ_{3}}} \\ = \pm \frac{1}{2} \sqrt{(1 + a_{z}) (1 \pm \frac{| χ_{4} |}{\sqrt{χ_{3}}})} \end{matrix}

(18)

Consequently, the most important work is to solve the simplified form of $χ_{3}$ . We find out that $χ_{3}$ can be expanded by $m_{x}^{2}, m_{y}^{2}, m_{z}^{2}, m_{x} m_{y}, m_{x} m_{z}, m_{y} m_{z}$ and their corresponding coefficients, such that

\begin{array}{l} χ_{3} = m_{x}^{2} {a_{x}^{4} + 2 a_{x}^{2} [a_{y}^{2} - {(1 + a_{z})}^{2}] + {[a_{y}^{2} + {(1 + a_{z})}^{2}]}^{2}} \\ + m_{y}^{2} {a_{x}^{4} + 2 a_{x}^{2} [a_{y}^{2} + {(1 + a_{z})}^{2}] + {[a_{y}^{2} - {(1 + a_{z})}^{2}]}^{2}} \\ + m_{z}^{2} {4 (a_{x}^{2} + a_{y}^{2}) {(1 + a_{z})}^{2}} \\ + m_{x} m_{y} {- 8 a_{x} a_{y} {(1 + a_{z})}^{2}} \end{array}

\begin{array}{l} + m_{x} m_{z} {4 a_{x} (1 + a_{z}) [a_{x}^{2} + a_{y}^{2} - {(1 + a_{z})}^{2}]} \\ + m_{y} m_{z} {4 a_{y} (1 + a_{z}) [a_{x}^{2} + a_{y}^{2} - {(1 + a_{z})}^{2}]} \end{array}

(19)

A frequently appeared item is pre-calculated by

\begin{array}{l} a_{x}^{2} + a_{y}^{2} - {(1 + a_{z})}^{2} \\ = 1 - a_{z}^{2} - 1 - a_{z}^{2} - 2 a_{z} \\ = - 2 a_{z} (1 + a_{z}) \end{array}

(20)

For $m_{x}^{2}$ , its coefficient can be simplified using equation (20) and will arrive at

\begin{array}{l} a_{x}^{4} + 2 a_{x}^{2} [a_{y}^{2} - {(1 + a_{z})}^{2}] + {[a_{y}^{2} + {(1 + a_{z})}^{2}]}^{2} \\ = {[a_{x}^{2} + a_{y}^{2} - {(1 + a_{z})}^{2}]}^{2} + 4 a_{y}^{2} {(1 + a_{z})}^{2} \\ = 4 (a_{z}^{2} + a_{y}^{2}) {(1 + a_{z})}^{2} \end{array}

(21)

Using the same methodology, $χ_{3}$ can be simplified with

\begin{matrix} χ_{3} = m_{x}^{2} {4 (a_{y}^{2} + a_{z}^{2}) {(1 + a_{z})}^{2}} \\ + m_{y}^{2} {4 (a_{x}^{2} + a_{z}^{2}) {(1 + a_{z})}^{2}} \\ + m_{z}^{2} {4 (a_{x}^{2} + a_{y}^{2}) {(1 + a_{z})}^{2}} \\ + m_{x} m_{y} {- 8 a_{x} a_{y} {(1 + a_{z})}^{2}} \\ + m_{x} m_{z} {- 8 a_{x} a_{z} {(1 + a_{z})}^{2}} \\ + m_{y} m_{z} {- 8 a_{y} a_{z} (1 + a_{z})^{2}} \\ = (1 + a_{z})^{2} [\begin{array}{l} 4 (1 - a_{x}^{2}) m_{x}^{2} + 4 (1 - a_{y}^{2}) m_{y}^{2} + 4 (1 - a_{z}^{2}) m_{z}^{2} \\ - 8 a_{x} a_{y} m_{x} m_{y} - 8 a_{x} a_{z} m_{x} m_{z} - 8 a_{y} a_{z} m_{y} m_{z} \end{array}] \\ = {(1 + a_{z})}^{2} (4 - 4 α^{2}) \end{matrix}

(22)

where

α = a_{x} m_{x} + a_{y} m_{y} + a_{z} m_{z} \in (- 1, 1)

(23)

which indicates that

χ_{3}

is always positive as the measurement input varies.

Quaternion selector and calculation steps

Looking back to equation (8), we could easily know the maximum numbers of all the solutions should be 4 × 2 = 8.³¹ Recalling that in the last subsection, we have mathematically ignored one set of q₀ that would have distribution in complex space. Thus, there are only four remaining quaternion solutions to equation (8).

However, not all the solutions can match equation (6) properly. As can be given with equation (6)

\begin{matrix} A^{b} \cdot M^{b} = (C_{r}^{b} A^{r}) \cdot (C_{r}^{b} M^{r}) = {(A^{r})}^{⊤} {(C_{r}^{b})}^{⊤} C_{r}^{b} M^{r} \\ = {(A^{r})}^{⊤} C_{b}^{r} C_{r}^{b} M^{r} = {(A^{r})}^{⊤} M^{r} = A^{r} \cdot M^{r} \end{matrix}

(24)

M^{r}

in equation (6) is constrained as a dynamic vector rather than a constant.³² This finding yields

m_{D} = A^{r} \cdot M^{r} = A^{b} \cdot M^{b} = a_{x} m_{x} + a_{y} m_{y} + a_{z} m_{z} = α

(25)

Here, m_N is a time-varying variable, which can be given by

m_{N} = \sqrt{1 - α^{2}}

(26)

With the above derivations, we can form the verification function and the task is to minimize the verification function giving a set of determined quaternions, such that

\underset{‖ q ‖ = 1}{argmin} {\frac{1}{2} [{(m_{N} - {\tilde{m}}_{N})}^{2} + {(m_{D} - {\tilde{m}}_{D})}^{2}]}

(27)

where, m_N and m_D denote the reference information of magnetometer calculated by equations (25) and (26).

{\tilde{m}}_{N}

and

{\tilde{m}}_{N}

stand for the estimated reference information and can be calculated by

{\begin{array}{l} (1 - 2 q_{2}^{2} - 2 q_{3}^{2}) m_{x} \\ + 2 (q_{1} q_{2} - q_{0} q_{3}) m_{y} \\ + 2 (q_{1} q_{3} + q_{0} q_{2}) m_{z} = {\tilde{m}}_{N} \\ 2 (q_{1} q_{3} - q_{0} q_{2}) m_{x} \\ + 2 (q_{0} q_{1} + q_{2} q_{3}) m_{y} \\ + (1 - 2 q_{1}^{2} - 2 q_{2}^{2}) m_{z} = {\tilde{m}}_{D} \end{array}

(28)

which can be extracted from equation (7). Once we get a set of quaternions, the optimal one will make the verification function f be the minimum. In this way, the optimal quaternion can be selected. Finally, the calculation procedures are given below:

Algorithm 1 Calculation procedure of the proposed method.

Require: one group of measurements from accelerometer and magnetometer (

A^{b}, M^{b}

1: Calculate $α, χ_{3}, χ_{4}$ using (equations (23), (22), and (17).

2: Calculate q₃ using equation (18).

3: Calculate $q_{0}, q_{1}, q_{2}$ using equation (14).

4: Calculate all four quaternions and verify them using quaternion selector.

5: If possible, calculate Euler angles using equation (34).

Experiments, simulations, and results

The MTi Xsens Inertial Measurement Unit (IMU) with raw inertial and geomagnetic outputs along with the real-time attitude is used in order to evaluate findings proposed in previous sections. Manufactory standard of Xsens IMU is listed in Table 1. We use MATLAB r2015a to show and compare all the experimental results. Some of the experiments are carried out on embedded platform, while others are verified using the MATLAB software.

Table 1.

Specifications of Xsens Inertial Measurement Unit (IMU).

Specification	Value
Vibration and shock	MIL STD-202; 2000 g
Sampling frequency	10 kHz/channel (60 kS/s)
Output frequency	Up to 2 kHz
Latency	<2 ms
Geomagnetic output	Up to 400 Hz
Standard full range gyro	450^°/s (1000 ^°/s available as an option)
Standard full rangeaccelerometer	50 $m / s^{2}$ (150 $m / s^{2}$ available as an option)
In-run bias stability gyro	10/h
Bandwidth gyro	415 Hz
Bandwidth acc	375 Hz

Static test

In this subsection, we put the IMU on an magnetism-free three-axis rotary platform, which is shown in Figure 1. We use $M^{r} = {(0.8, 0, - 0.6)}^{⊤}$ as the reference vector of the magnetometer according to the local magnetic declination. The reference Euler angles are obtained from the internal AHRS of the IMU. The motion is produced using the motion generator. Then the signal is transmitted to the rotary platform. With raw data from accelerometer and magnetometer, the corresponding attitude angles are calculated with different algorithms. Figure 2 shows the details. From this figure, we can see that the roll and pitch determination performances of QUEST, FQA, and the proposed method are basically the same. This verifies the correctness of the proposed method for determining gravity direction. However, the yaw performances indicate that the proposed method produces much more accurate yaw determination. This reason is that QUEST and FQA highly require the magnetometer’s reference vector, which may not accurate in real applications. For this case, the settled magnetometer’s reference vector $M^{r} = {(0.8, 0, - 0.6)}^{⊤}$ is quite different from the real one of the operation position. Hence, we can see that there is an constant offset between the yaw determination of QUEST, FQA, and the reference angle. This also shows that the proposed method can accurately determine the yaw angle without magnetometer’s reference information.

Figure 1.

The IMU and magnetism-free three-axis rotary platform for experimental validation.

Figure 2.

Static attitude test results from different algorithms.

Dynamic test

The AMC is also used to give attitude estimates for mobile platforms. In such applications, external acceleration may lower the signal–noise ratio (SNR) of the accelerometer’s output.²¹ Consequently, low-pass filter (LPF) is usually adopted to filter the external acceleration, which makes the gravity sensing from the accelerometer more reliable. In this subsection, we present the dynamic performances of the proposed method and other representative methods. A quadrotor is chosen as the experimental platform (see Figure 3). Raw data sequence is obtained during a flight in real world. In this experiment, the magnetometer’s reference vector is the same with that in last subsection. The cut-off frequency of the LPF is set to 10 Hz empirically so that any external acceleration with frequency of over 10 Hz would be filtered. Figure 4 shows the determination results. We can see that the proposed algorithm can also effectively determine the attitude angles. Accordingly, the yaw estimates from the proposed method are better than those from QUEST and FQA. To make the results more clear, the attitude error is plotted in Figure 5. The mean error angles of all these algorithms are given in the Table 2.

Figure 3.

A quadrotor is chosen as the experimental platform. The MTi Xsens IMU is firmly attached to the vehicle.

Figure 4.

Dynamic attitude determination results.

Figure 5.

Attitude errors of different algorithms in dynamic mode.

Table 2.

Mean attitude errors in dynamic mode.

Algorithm	Roll	Pitch	Yaw
FQA	${4.2335}^{°}$	${5.2077}^{°}$	${177.82}^{°}$
QUEST	${4.2277}^{°}$	${5.1865}^{°}$	${178.37}^{°}$
Proposed method	${4.1414}^{°}$	${4.0069}^{°}$	${1.7143}^{°}$

FQA: factored quaternion algorithm.

Calculation accuracy

Using the data set in the dynamic test, the calculation errors are calculated, which are depicted in Figure 6. We can see that the overall calculation accuracy is about $3 \times 10^{- 15}$ , which denotes a very good accuracy. This shows that although the proposed method uses analytic procedures to calculate the attitude, the basic accuracy is still satisfactory, which again proves the correctness of the proposed method.

Figure 6.

CMSEs of the proposed method.

Figure 7.

Time consumption of various algorithms.

Time consumption

We use the random simulated data set to evaluate the time consumption of different algorithms (See Figure 7). The results show that the time consumption of the proposed method is the least and much less than those of QUEST and FQA. The reason is that QUEST undertakes many matrix operations like inversion and eigenvalue decomposition, while FQA uses many trigonometric functions to achieve attitude determination. The above computations require a large amount of time consumption making them time-costly. This shows that not only the proposed method can give accurate attitude determination, but it takes the least time consumption as well.

Compared with iterative method

The IGNA for attitude determination proposed by Tian et al.²⁶ is used for comparison. For IGNA, the maximum iteration number is set to 50. Tables 3 and 4 show the calculation mean-squared errors (CMSEs) of IGNA and the proposed method respectively. CMSE is introduced for performance demonstration. Mathematical forms of the CMSEs are given as follows:

{\begin{array}{l} C M S E_{1} = \frac{1}{N} \sum_{i = 1}^{N} {(2 (q_{1} q_{3} - q_{0} q_{2}) - a_{x})}_{i}^{2} \\ C M S E_{2} = \frac{1}{N} \sum_{i = 1}^{N} {(2 (q_{2} q_{3} + q_{0} q_{1}) - a_{y})}_{i}^{2} \\ C M S E_{3} = \frac{1}{N} \sum_{i = 1}^{N} {(1 - 2 q_{1}^{2} - 2 q_{2}^{2} - a_{z})}_{i}^{2} \\ C M S E_{4} = \frac{1}{N} \sum_{i = 1}^{N} {(\begin{array}{l} 2 m_{x} (q_{1} q_{2} + q_{0} q_{3}) \\ + m_{y} (1 - 2 q_{1}^{2} - 2 q_{3}^{2}) \\ + 2 m_{z} (q_{2} q_{3} - q_{0} q_{1}) \end{array})}_{i}^{2} \end{array}

(29)

where i denotes the index of the measurement and N denotes the amount of all the measurements.

Table 3.

Calculation mean-squared errors (CMSEs) of improved Gauss–Newton algorithm (IGNA).

	Set 1 (1879)	Set 2 (1392)	Set 3 (1766)	Set 4 (1506)	Set 5 (1741)	Set 6 (8798)
CMSE ₁	3.8194⁻¹¹	8.5843⁻⁰⁹	3.5222⁻⁰⁸	2.1490⁻¹⁰	1.0998⁻⁰⁸	3.5986⁻¹²
CMSE ₂	1.1789⁻¹¹	1.9939⁻⁰⁸	4.2614⁻⁰⁸	9.4193⁻¹¹	7.7725⁻⁰⁹	1.1856⁻¹²
CMSE ₃	1.7320⁻¹⁴	4.9535⁻¹²	8.6357⁻¹²	3.7915⁻¹⁴	2.9019⁻¹²	4.7699⁻¹⁶
CMSE ₄	1.7918⁻⁰⁷	8.5879⁻⁰⁵	2.9736⁻⁰⁴	2.8618⁻⁰⁶	6.4282⁻⁰⁵	4.0537⁻⁰⁸

Table 4.

Calculation mean-squared errors (CMSEs) of the proposed method.

	Set 1 (1879)	Set 2 (1392)	Set 3 (1766)	Set 4 (1506)	Set 5 (1741)	Set 6 (8798)
CMSE ₁	5.0671⁻¹⁶	3.0745⁻¹³	3.6899⁻⁰⁸	4.9878⁻¹⁰	2.1158⁻¹¹	8.9257⁻¹²
CMSE ₂	1.3834⁻¹⁷	4.1229⁻¹⁴	2.2106⁻⁰⁸	2.0905⁻⁰⁹	1.5281⁻¹³	1.6579⁻¹⁰
CMSE ₃	5.0310⁻²⁰	2.4752⁻¹⁸	6.4099⁻¹³	2.5643⁻¹⁵	2.6136⁻¹⁵	1.7098⁻¹⁵
CMSE ₄	7.0049⁻¹²	4.0907⁻⁰⁸	3.9005⁻⁰³	3.3254⁻⁰⁴	2.0270⁻⁰⁷	1.2493⁻⁰⁹

The numbers in the brackets behind captions indicate the sizes of different simulated data sets. These results reflect that compared with IGNA the proposed method owns the best CMSEs even IGNA is carried out for 50 times per sampling period. The simulation also tests the time consumption performances of IGNA and the proposed method, which are given in Tables 5 and 6.

Table 5.

Time consumption of improved Gauss–Newton algorithm (IGNA).

	Set 1 (1879)	Set 2 (1392)	Set 3 (1766)	Set 4 (1506)	Set 5 (1741)	Set 6 (8798)
Time	121.1 s	82.3 s	76.2 s	99.2 s	85.4 s	406.5 s
Time per sampling period	0.06 s	0.06 s	0.04 s	0.066 s	0.05 s	0.046 s
Maximum processing frequency	16.7 Hz	16.7 Hz	25 Hz	15.2 Hz	20 Hz	21.7 Hz

Table 6.

Time consumption of the proposed method.

	Set 1 (1879)	Set 2 (1392)	Set 3 (1766)	Set 5 (1506)	Set 6 (1741)	Set 7 (8798)
Time	0.274 s	0.259 s	0.265 s	0.224 s	0.304 s	1.23 s
Time per sampling period	1.46^−04s	1.857^−04s	1.50^−04s	1.49^−04s	1.75^−04s	1.40^−04s
Maximum processing frequency	6849 Hz	5385 Hz	6667 Hz	6711 Hz	5714 Hz	7142 Hz

Tables 5 and 6 show the time consumption performances of various algorithms. Since IGNA is an iterative method, it owns the worst time consumption performance. The proposed method’s quaternion solution can substitute for the previous iterative ones for much faster speed in the integrated sensor fusion involving MEMS gyroscope.

Conclusion

The attitude determination based on AMC can be reduced to the Wahba’s problem or a set of nonlinear equations. Although some iterative methods have been developed to solve the problem, there are basically no analytic approach. Yet Wahba’s problem requires the reference information of sensors. Interestingly, after reformulating the original nonlinear equations, not only it no longer relies on the reference vector but the quaternion solutions to it can be found. These analytic solutions are successfully obtained and simplified in this paper. Since not all the quaternions are qualified for the specific problem, a quaternion selector is designed to make sure the final output quaternions are optimal. Experiments verified the efficiency and good performances of the proposed method especially its low time consumption with respect to several representative methods. We hope that the proposed method would be of benefit to the related attitude determination applications.

Footnotes

Acknowledgement

This paper is presented in memorial of author Jin Wu’s grandfather for his former guidance.

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work is supported by the National Natural Science Funds of China (41604025), the Key Laboratory of Precise Engineering and Industry Surveying of National Administration of Surveying, Mapping and Geoinformation (PF2015-11). State Key Laboratory of Geodesy and Earth's Dynamics (Institute of Geodesy and Geophysics, CAS) Grant No. SKLGED2018-3-2-E.

Appendix 1: Basic knowledge of rotation

In this paper, quaternions are used for representing attitude of an object. In general, a quaternion is defined over the Hamilton Space $H$ and can be given by³³ (30)

q = q_{0} + i \cdot q_{1} + j \cdot q_{2} + k \cdot q_{3}

where, the unit imaginary number i, j, k are orthogonal with each other. One quaternion can be normalized using the criteria shown below (31)

q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2} = 1

Also, there is an convertion between the DCM and quaternions. In analytic geometry, DCM is a classical representation of the rotation which can be written as (32)

C = (\begin{matrix} c ψ c θ & s ψ c θ & - s θ \\ - s ψ c γ + c ψ s θ s γ & c ψ c γ + s ψ s θ s γ & c θ s γ \\ s ψ s θ + c ψ c θ c γ & - c ψ s γ + s ψ s θ c γ & c θ c γ \end{matrix})

where,

C

is the DCM from the geographical frame to the human frame. c and s are the abbreviations of the cosine function and the sine function, respectively. Here, the NED coordinate system is used as the geographical frame.

θ, ψ, γ

indicate the pitch, yaw, and roll angles of the given object, respectively.

One DCM’s inverse equals to the DCM’s transpose (33)

{(C)}^{- 1} = {(C)}^{⊤}

According to quaternions theory,³⁴ the DCM can be written with quaternions as well. And the relationship between the DCM and $θ, ψ, γ$ can be given by (34)

\begin{matrix} θ = \arctan \frac{- C_{31}}{\sqrt{C_{32}^{2} + C_{33}^{2}}} \\ ψ = \arctan \frac{C_{21}}{C_{11}} \\ γ = \arctan \frac{C_{32}}{C_{33}} \end{matrix}

where,

C_{ij}

represents the element in ith row and jth column of the DCM. When

θ \to \pm \frac{π}{2}

C_{11}, C_{21}, C_{32}, C_{33}

will approach 0 at the same time which will cause the undetermined singularity of the

\arctan

function.

References

Ligorio

Sabatini

A novel Kalman filter for human motion tracking with an inertial-based dynamic inclinometer.

Biomed Eng IEEE Trans 2015; 62: 2033–2043.

Supej

3D measurements of alpine skiing with an inertial sensor motion capture suit and GNSS RTK system.

J Sports Sci 2010; 28: 759–769.

Tian

Hamel

Tan

Accurate human navigation using wearable monocular visual and inertial sensors.

IEEE Trans Instrument Measur 2014; 63: 203–213.

Zhou

Human motion tracking for rehabilitation—a survey.

Biomed Signal Process Contr 2008; 3: 1–18.

Chang

Qin

In-motion initial alignment for odometer-aided strapdown inertial navigation system based on attitude estimation.

IEEE Sens J 2017; 17: 766–773.

Zhou

Zhang

et al . Integrated navigation system for a low-cost quadrotor aerial vehicle in the presence of rotor influences. J Survey Eng 2017; 143: 05016006.

Zhou

Liu

et al . Equality constrained robust measurement fusion for adaptive Kalman-filter-based heterogeneous multi-sensor navigation. IEEE Trans Aerosp Electron Syst 2013; 49: 2146–2157.

Yun

Bachmann

Design, implementation, and experimental results of a quaternion-based Kalman filter for human body motion tracking.

Robot IEEE Trans 2006; 22: 1216–1227.

Chang

Modified unscented quaternion estimator based on quaternion averaging.

J Guid Control Dynam 2014; 37: 305–308.

10.

Markley

Mortari

New developments in quaternion estimation from vector observations. Adv Astronaut Sci 2000; 106: 373–393.

11.

Chang

Chen

Initial alignment by attitude estimation for strapdown inertial navigation systems. IEEE Trans Instrument Measur 2015; 64: 784–794.

12.

Wahba

A least squares estimate of satellite attitude.

SIAM Rev 1965; 7: 409.

13.

Shuster

SD.

Three-axis attitude determination from vector observations.

J Guid Contr Dyn 1981; 4: 70–77.

14.

Markley

FL.

Attitude determination using vector observations—a fast optimal matrix algorithm. J Astronaut Sci 1993; 41: 261–280.

15.

Mortari

ESOQ: a closed-form solution to the Wahba problem. J Astronaut Sci 1997; 45: 195–204.

16.

Zhou

Gao

et al.

Fast linear quaternion attitude estimator using vector observations. IEEE Trans Automat Sci Eng 2018; 15(1): 307–319.

17.

Yang

Zhou

An analytic solution to Wahbas problem.

Aerosp Sci Technol 2013; 30: 46–49.

18.

Yang

Attitude determination using Newton’s method on Riemannian manifold. Proc Inst Mech Eng Part G: J Aerosp Eng 2015; 229(14): 2737–2742.

19.

Zhou

et al . Attitude determination using a single sensor observation: analytic quaternion solutions and property discussion. IET Sci Measur Technol 2017; 11(3): 731–739.

20.

Turkoglu

Wahl

EL.

Wahba’s problem: a combined binary search and bisection approach. In: AIAA guidance, navigation, and control conference, January 9 – 13, 2017, Grapevine, Texas. p. 1032.

21.

Zhou

Chen

et al . Fast complementary filter for attitude estimation using low-cost MARG sensors. IEEE Sens J 2016; 16: 6997–7007.

22.

Wertz

Spacecraft attitude determination and control, Springer, Dordrecht: Springer, 1978. ISBN 9027709599.

23.

Bar-Itzhack

Harman

RR.

Optimized TRIAD algorithm for attitude determination.

J Guid Contr Dyn 1997; 20: 208–211.

24.

Sabatini

AM.

Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing.

IEEE Trans Biomed Eng 2006; 53: 1346–1356.

25.

Madgwick

SOH

Harrison

AJL

Vaidyanathan

Estimation of IMU and MARG orientation using a gradient descent algorithm. IEEE international conference on rehabilitation robotics, ETH Zurich Science City, Switzerland, June 29 – July 1, 2011.

26.

Tian

Wei

Tan

An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments.

IEEE Trans Neural Syst Rehabilit Eng 2013; 21: 254–264.

27.

Fourati

Manamanni

Afilal

et al . Complementary observer for body segments motion capturing by inertial and magnetic sensors. IEEE/ASME Trans Mechatr 2014; 19: 149–157.

28.

Suh

YS.

Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration.

IEEE Trans Instrument Measur 2010; 59: 3296–3305.

29.

Jie

Xiaoming

Jun

et al . Calibration of triaxial MEMS vector field measurement system. IET Sci Measur Technol 2014; 8: 601–609.

30.

Marins

Yun

Bachmann

et al . An extended Kalman filter for quaternion-based orientation estimation using MARG sensors. In: Proceedings 2001 IEEE/RSJ international conference on intelligent robots and systems expanding the societal role of robotics in the next millennium (Cat No01CH37180), USA, 2001, p. 4.

31.

Liu

Erne

Algebraic geometry and arithmetic curves. Oxford, UK: Oxford University Press, 2002.

32.

Markley

Crassidis

JL.

Fundamentals of spacecraft attitude determination and control, volume 33. Springer, Dordrecht: Springer, 2014.

33.

Hamilton

WR.

Lectures on quaternions, Dublin, Ireland: Hodges and Smith. 1853.

34.

Britting

KR.

Inertial navigation systems analysis. Norwood, Massachusetts: Artech House Publishers, 2010.