A Customized Extended Kalman Filter for Removing the Impact of the Magnetometer’s Measurements on Inclination Determination

Abstract

Normally, a three-dimensional orientation determination algorithm that is used in a magnetic and inertial measurement unit calculates the inclination (including both the pitch and roll) of rigid bodies by fusing the measurements of the gyroscope, as well as the measurements of both the accelerometer and the magnetometer. The measurements of the magnetometer can be helpful in improving the inclination estimation accuracy; however, once the measurements of the magnetometer are disturbed by ferromagnetic materials, the inclination estimation accuracy could be significantly decreased. Hence, a better approach should be followed in terms of not employing the measurements of the magnetometer for inclination determination. In order to achieve this goal, the component of the measurement of the magnetometer that is used for the improvement of the inclination estimation accuracy, along with the measurement of the accelerometer at each sampling time instant, is abandoned. Consequently, the remaining component of the measurement of the magnetometer, which is perpendicular to the measurement of the accelerometer, is used for the azimuth determination. After applying this process, the extended Kalman filter (EKF) is proposed for the inclination and azimuth estimations. Through experiments, the EKF is compared with three algorithms that were recently proposed with the same objective as this work, and the extracted outcomes show that the EKF approach clearly outperforms these three algorithms.

1. Introduction

This paper focuses on the decoupling problem faced by three-dimensional (3D) orientation determination algorithms (ODAs). ODAs are employed in magnetic and inertial measurement units (MIMUs) for determining their orientation when attached on moving rigid bodies, which is achieved by fusing the outputs of their magnetic, angular rate, and gravity (MARG) sensors. In fact, the basic principle of ODAs can be explained in a few words: the orientation is first predicted by relying on the measurement of the angular rate sensor and is then revised (or we can say “updated”) by introducing the measurements of the magnetic and gravity sensors. ODAs mainly differ by their different orientation revision strategies. The word “coupling” indicates that the measurement of the magnetic sensor participates in the calculation of the inclination angles, which causes the magnetic disturbance imposed on the magnetic sensor to degrade the estimation accuracy for the pitch and roll. Hence, the decoupling problem refers to understanding how to eliminate the negative effect of magnetic disturbance on the determination of inclination angles, including the pitch and roll. We present some novel modifications to the standard extended Kalman filter (EKF) for problem solving, showing that the EKF is still the most promising, powerful, and reliable ODA.

MIMUs are widely used in several applications where the determination of the inclination (including the pitch and roll) and azimuth (also called yaw or heading) of rigid bodies is required. Among the above-mentioned applications, the most conventional is the movement analysis of human body segments [1,2], followed by the inclination and azimuth sensing of small and medium-sized machines (usually autonomous equipment) flying through the air but near the ground [3], sailing under water [4,5], and driving on the ground [6]. Despite the fact that other methods, such as vision-based methods and GNSS-based methods, can also be applied for determining the inclination and azimuth, MIMUs have their unique superiorities, including their low cost, small size, light weight, and non-reliance on external artificial sources.

As mentioned, the key sensors within MIMUs are the tri-axis gyroscope (angular rate sensor), the tri-axis accelerometer (gravity sensor), and the tri-axis magnetometer (magnetic sensor), in addition to some other components such as the analog-to-digital converter, the temperature sensor used for sensor temperature drift compensation, and the micro-processor. A sensor fusion algorithm, such as a 3D ODA, is required in order to calculate the inclination and azimuth of the rigid body to which an MIMU is attached. Quaternions are usually selected for 3D orientation representations and are estimated in real time by applying an ODA, whereas the Euler angles are usually preferred when a visual presentation is needed. On top of that, if the latter takes place, then the quaternions should be converted to Euler angles.

ODAs can be further classified into two categories, i.e., Kalman filters and complementary filters. As far as Kalman filters are concerned, several modifications can be employed. More specifically, the linear Kalman filter (LKF) [7,8] and its extended version [9,10,11] have been reported. The latter incorporates the estimation of the gyroscope drift, while the former does not. Some other configurations have also been actively examined, including the EKF [12,13,14,15], the multiplicative extended Kalman filter (MEKF) [16,17,18,19,20], the unscented Kalman filter (UKF) [21,22], the cubature Kalman filter (CKF) [23], and the particle filter (PF) [24]. The structures of the EKFs proposed by [12,13,14,15] are exactly the same, except that the EKFs in [12] and [13] estimate any sensor bias, while the EKF in [15] estimates the sensor bias in addition to the orientation, and the EKF in [14] simplifies the Kalman gain matrix to decrease the computation time without reducing the orientation estimation accuracy. Different from the EKF, which realizes the linearization by using the Taylor expansion, the UKF and CKF realize the linearization by using unscented transformation and cubature transformation, respectively. The authors of the UKF claim that the approximations given by the UKF are accurate to the third order for Gaussian inputs for all nonlinearities, while they are accurate to at least the second order for non-Gaussian inputs [25]. The authors of the CKF claim that the CKF is more accurate and more principled in mathematical terms than the UKF [26]. The UKF, CKF, and PF are rarely used in MIMUs due to their high computational burden and relatively poor performance improvement compared to the EKF [27,28]. On the other hand, the MEKF is usually considered a first-order approximation to the EKF, and hence its accuracy is lower than that of the EKF [29]. The main difference among the various complementary filters is their diverse selection of single-frame inclination and azimuth determination algorithms. For instance, various algorithms have been investigated, including the Levenberg–Marquard algorithm [30], the factored quaternion algorithm [31], and the gradient descent algorithm [32,33,34,35]. It is interesting to note that complementary filters do not take into account the noise characteristics of the sensor measurements. As a result, their accuracy is normally lower than that of Kalman filters [36,37,38]. Moreover, the LKF exhibits a similar structure to complementary filters, except for the noise characteristics of the sensor measurements. In recent years, deep neural networks have also been introduced for inclination and azimuth determination, showing remarkable performances in some cases [39] due to their great success in pattern recognition.

It is well known that the measurements of the accelerometer are only useful for inclination determination, while the measurements of the magnetometer are mainly employed for azimuth determination. However, most ODAs take into account the measurements of both the accelerometer and magnetometer for inclination determination. Therefore, the inclination determination accuracy could be severely affected by the magnetic disturbance induced by ferromagnetic materials around MIMUs. In on-ground environments, particularly indoor environments, various hard and soft magnetic materials and excitation sources can be found, especially around the objects whose inclination and azimuth angles need to be determined, such as cars, security doors, mobile phones, metal tables and desks, and even whole buildings, which are obviously supported by ferroconcrete. MIMUs also contain magnetic materials and excitation sources. All these magnetic materials and excitation sources can distort the geomagnetic field. This means that large unmodeled magnetic disturbances occur more frequently than unmodeled acceleration disturbances; therefore, preventing magnetic disturbances from influencing the calculation of inclination angles will hopefully improve the estimation performance and hence is of practical importance. As a result, the goal of this work is to solve this problem. In the literature, various algorithms have been proposed in order to deal with this issue [40,41,42]. The basic idea of the extended complementary filter proposed in Ref. [40] is to correct the gyroscope drift by using the calculation results of two error terms, which are derived from the cross product of the measured and reference vectors for gravity and the geomagnetic field before integrating the gyroscope measurement to obtain the final orientation estimate. The four-parameter complementary filter presented in Ref. [41] is based on a new attitude representation, where 4D parameters (a 3D unit vector and one angle quantity) are used. The authors claim that the proposed representation has an intuitive interpretation and that the proposed filter has a separation property (magnetic disturbance does not affect the pitch and roll angle estimation). The fast complementary filter designed in Ref. [42] is a two-step filter. The pitch and roll are estimated in the first step, while the azimuth is estimated in the second step. Once it is recognized that a magnetic disturbance has emerged and degraded the accuracy of the magnetometer, the second step will be skipped so as to prevent the magnetic disturbance from affecting the computation of the pitch and roll. Obviously, a recognizer should be included in the designed filter; however, the recognizer cannot recognize all magnetic disturbances; hence, if it incorrectly recognizes a disturbance, the magnetic disturbance can still heavily affect the estimation accuracy of the pitch and roll.

From our experimental analysis, we found that the above-mentioned algorithms still present significant problems. Along these lines, we present various creative modifications to the EKF for problem solving. With the newly formulated measurement model and its error covariance matrix, the creatively modified EKF is capable of solving the decoupling problem, so that the negative effect of magnetic disturbance on the calculation of the inclination is eliminated. Experiments were carried out demonstrating that the modified EKF outperforms recently published algorithms. From our analysis, we draw the conclusion that the EKF is still the preferred algorithm when various practical problems are encountered.

The remainder of this paper is organized as follows: Section 2 elaborates on the details of the state transition model, as well as the measurement model that was used for the EKF. Section 3 provides the experimental comparison results between the EKF and three other algorithms that also aim to overcome the impact of magnetic disturbance on the inclination determination. Finally, Section 4 concludes this paper.

2. Analysis of the State Transition and Measurement Models That Were Used for the Extended Kalman Filter

2.1. Sensor Model

The outputs of the three-axis gyroscope, three-axis accelerometer, and three-axis magnetometer at the time instant $k{T}_s$ (where $T_s$ is the sampling period) are denoted as follows:

$$\left\{\begin{array}{l}{\mathit{\omega }}_k={\mathit{\omega }}_k^0+{\mathit{\epsilon }}_k^{\omega }\hfill \\ {\mathit{g}}_k={\mathit{g}}_k^0+{\mathit{\eta }}_k^g+{\mathit{\epsilon }}_k^g\hfill \\ {\mathit{m}}_k={\mathit{m}}_k^0+{\mathit{\eta }}_k^m+{\mathit{\epsilon }}_k^m\hfill \end{array}\right.$$

(1)

where k is a simple representation of “$k{T}_s$”, ${\mathit{\omega }}_k^0$ is the true angular velocity, and ${\mathit{\omega }}_k$ is its measurement provided by the gyroscope. Additionally, ${\mathit{g}}_k^0$ is the true unnormalized gravity represented in the body coordinate system, ${\mathit{g}}_k$ is its respective measurement given by the accelerometer, ${\mathit{m}}_k^0$ is the Earth’s true unnormalized magnetic field represented in the body coordinate system, and ${\mathit{m}}_k$ is its measurement value given by the magnetometer. Moreover, ${\mathit{\epsilon }}_k^{\omega }$, ${\mathit{\epsilon }}_k^g$, and ${\mathit{\epsilon }}_k^m$ are assumed to be uncorrelated white Gaussian measurement noise, with a null mean and the covariance matrices ${\mathit{\Sigma }}_{\omega }={\mathsf{\sigma }}_{\omega }^2\mathbf{I}$, ${\mathit{\Sigma }}_g={\mathsf{\sigma }}_g^2\mathbf{I}$, and ${\mathit{\Sigma }}_m={\mathsf{\sigma }}_m^2\mathbf{I}$ (where $\mathbf{I}$ is the identity matrix), respectively. Finally, ${\mathit{\eta }}_k^g$ and ${\mathit{\eta }}_k^m$ are the acceleration and magnetic disturbances, respectively.

2.2. State Transition Model

The continuous-time rigid-body angular motion is described by employing the following expression [43]:

$$\dot{\mathit{q}}=\frac{1}{2}\left[\begin{array}{cc}[{\mathit{\omega }}^0\times ]& {\mathit{\omega }}^0\\ -{({\mathit{\omega }}^0)}^T& 0\end{array}\right]\mathit{q}$$

(2)

where ${\mathit{\omega }}^0={[{\omega }_x^0,{\omega }_y^0,{\omega }_z^0]}^T$, and the anti-symmetric matrix $[{\mathit{\omega }}^0\times ]$ is constructed as follows:

$$[{\mathit{\omega }}^0\times ]=\left[\begin{array}{ccc}0& -{\omega }_z^0& {\omega }_y^0\\ {\omega }_z^0& 0& -{\omega }_x^0\\ -{\omega }_y^0& {\omega }_x^0& 0\end{array}\right]$$

(3)

where ${\omega }_x^0$, ${\omega }_y^0$, and ${\omega }_z^0$ are the true angular velocities along the X, Y, and Z axes of the body coordinate system, respectively. Furthermore, $\mathit{q}={[{\mathit{e}}^T,q_0]}^T={[q_1,q_2,q_3,q_0]}^T$ is the quaternion, whereas $\mathit{e}$ and $q_0$ are its vector and scalar part, respectively.

By discretizing Equation (2), we obtain the following expression:

$${\mathit{q}}_{k+1}=\exp \left(\frac{1}{2}\left[\begin{array}{cc}[{\mathit{\omega }}_k^0\times ]& {\mathit{\omega }}_k^0\\ -{({\mathit{\omega }}_k^0)}^T& 0\end{array}\right]\right){\mathit{q}}_k$$

(4)

where ${\mathit{q}}_k={[{\mathit{e}}_k^T,q_{0,k}]}^T={[q_{1,k},q_{2,k},q_{3,k},q_{0,k}]}^T$ is defined as the quaternion at the time instant $k{T}_s$.

After the measurement noise is taken into consideration, Equation (4) is rewritten as follows:

$${\mathit{q}}_{k+1}=\exp \left(\frac{1}{2}\left[\begin{array}{cc}[{\mathit{\omega }}_k\times ]& {\mathit{\omega }}_k\\ -{\mathit{\omega }}_k^T& 0\end{array}\right]\right){\mathit{q}}_k-\frac{T_s}{2}{\mathit{\Xi }}_k{\mathit{\epsilon }}_k^{\omega }$$

(5)

where ${\mathit{\Xi }}_k$ is expressed by the following equation:

$${\mathit{\Xi }}_k=\left[\begin{array}{c}[{\mathit{e}}_k\times ]+q_{0\cdot k}\mathbf{I}\\ -{\mathit{e}}_k^T\end{array}\right]$$

(6)

Equation (5) is actually the desired state transition model.

2.3. Measurement Model

The first measurement vector used here is ${\mathit{g}}_k$. The relation between ${\mathit{g}}_k$ and its reference unit vector ${\mathit{r}}_g={[0,0,-1]}^T$, which is represented in the global coordinate system (the local geographic coordinate system is selected, with its X, Y, and Z axes pointing north, west, and up, respectively), is expressed as follows:

$${\mathit{g}}_k={\mathit{g}}_k^0+{\mathit{\eta }}_k^g+{\mathit{\epsilon }}_k^g=‖{\mathit{g}}_k^0‖\mathit{C}({\mathit{q}}_k^0)\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]+{\mathit{\eta }}_k^g+{\mathit{\epsilon }}_k^g$$

(7)

where $‖{\mathit{g}}_k^0‖$ is the norm of ${\mathit{g}}_k^0$, ${\mathit{q}}_k^0$ is the true ${\mathit{q}}_k$, and the rotation matrix $\mathit{C}(\mathit{q})$ is defined by the following expression:

$$\mathit{C}(\mathit{q})=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_0^2& 2(q_1{q}_2+q_3{q}_0)& 2(q_1{q}_3-q_2{q}_0)\\ 2(q_1{q}_2-q_3{q}_0)& q_2^2-q_1^2-q_3^2+q_0^2& 2(q_2{q}_3+q_1{q}_0)\\ 2(q_1{q}_3+q_2{q}_0)& 2(q_2{q}_3-q_1{q}_0)& q_3^2-q_1^2-q_2^2+q_0^2\end{array}\right]$$

(8)

The second measurement vector is normally ${\mathit{m}}_k$. However, in order to remove the influence of the magnetometer’s measurement on the inclination determination, its component along ${\mathit{g}}_k$ is not used. As a result, the second selected measurement vector is

$${\mathit{m}}_k^{\prime }={\mathit{m}}_k-\frac{‖{\mathit{m}}_k‖}{‖{\mathit{g}}_k‖}{\mathit{g}}_k\cos ({\theta }_{gm})$$

(9)

where ${\theta }_{gm}$ is defined as the angle between ${\mathit{g}}_k$ and ${\mathit{m}}_k$.

The relation between ${\mathit{m}}_k^{\prime }$ and its reference unit vector ${\mathit{r}}_{m^{\prime }}={[1,0,0]}^T$, which is represented in the global coordinate system, is expressed as follows:

$${\mathit{m}}_k^{\prime }={{\mathit{m}}_k^{\prime }}^0+{\mathit{\eta }}_k^{m^{\prime }}+{\mathit{\epsilon }}_k^{m^{\prime }}=‖{\mathit{m}}_k^0‖\sin ({\theta }_{gm}^0)\mathit{C}({\mathit{q}}_k^0)\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]+{\mathit{\eta }}_k^{m^{\prime }}+{\mathit{\epsilon }}_k^{m^{\prime }}$$

(10)

where ${{\mathit{m}}_k^{\prime }}^0$, ${\mathit{\eta }}_k^{m^{\prime }}$, and ${\mathit{\epsilon }}_k^{m^{\prime }}$ are, respectively, the components of ${\mathit{m}}_k^0$, ${\mathit{\eta }}_k^m$, and ${\mathit{\epsilon }}_k^m$, and all are perpendicular to ${\mathit{g}}_k$. Furthermore, ${\mathit{\epsilon }}_k^{m^{\prime }}$ is assumed to be white Gaussian measurement noise, with a null mean and the covariance matrix ${\Sigma }_{m^{\prime }}={\mathsf{\sigma }}_{m^{\prime }}^2\mathbf{I}$; ${\theta }_{gm}^0$ is the true ${\theta }_{gm}$.

The proposed measurement model is constructed by stacking Equations (7) and (10) into one equation while ignoring the disturbance, as illustrated in the following expression:

$$\left[\begin{array}{c}\frac{{\mathit{g}}_k}{‖{\mathit{g}}_k‖}\\ \frac{{{\mathit{m}}^{\prime }}_k}{‖{\mathit{m}}_k‖\sin ({\theta }_{gm})}\end{array}\right]=\left[\begin{array}{cc}\mathit{C}({\mathit{q}}_k)& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& \mathit{C}({\mathit{q}}_k)\end{array}\right]\left[\begin{array}{c}{\mathit{r}}_g\\ {\mathit{r}}_{m^{\prime }}\end{array}\right]+\left[\begin{array}{c}\frac{{\mathit{\epsilon }}_k^g}{‖{\mathit{g}}_k‖}\\ \frac{{\mathit{\epsilon }}_k^{m^{\prime }}}{‖{\mathit{m}}_k‖\sin ({\theta }_{gm})}\end{array}\right]$$

(11)

In Equation (11), it is assumed that $\mathit{C}({\mathit{q}}_k^0)\approx \mathit{C}({\mathit{q}}_k)$, $‖{\mathit{g}}_k^0‖\approx ‖{\mathit{g}}_k‖$, $‖{\mathit{m}}_k^0‖\approx ‖{\mathit{m}}_k‖$, and ${\theta }_{gm}^0\approx {\theta }_{gm}$.

2.4. Calculation of the Error Covariance Matrices of the State Transition Model and the Measurement Model

The error covariance matrix of the state transition model given by Equation (5) is defined as follows:

$${\mathit{Q}}_k={\left(\frac{T_s}{2}\right)}^2{\mathsf{\sigma }}_{\mathit{\omega }}^2{\mathit{\Xi }}_k{\mathit{\Xi }}_k^T$$

(12)

By considering Equation (9), we can easily estimate that the error covariance matrix of the measurement model given by Equation (11) is formulated as follows:

$${\mathit{R}}_k=\left[\begin{array}{cc}\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}\mathrm{I}& -\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}\cot ({\theta }_{gm})\mathrm{I}\\ -\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}\cot ({\theta }_{gm})\mathrm{I}& \frac{{\mathsf{\sigma }}_{m^{\prime }}^2}{{‖{\mathit{m}}_k‖}^2{\left(\sin ({\theta }_{gm})\right)}^2}\mathrm{I}\end{array}\right]$$

(13)

where ${\mathsf{\sigma }}_{m^{\prime }}^2={\mathsf{\sigma }}_m^2+\frac{{‖{\mathit{m}}_k^2‖}^2}{{‖{\mathit{g}}_k^2‖}^2}{\mathsf{\sigma }}_g^2{\left(\cos \left({\theta }_{gm}\right)\right)}^2$. In order to derive Equation (13), it is assumed that ${\theta }_{gm}\approx {\theta }_{gm}^0$. However, the off-diagonal elements of ${\mathit{R}}_k$ that are provided by Equation (13) still render the estimation of the inclination susceptible to the measurements of the magnetometer; therefore, the matrix ${\mathit{R}}_k$ must be set as follows:

$${\mathit{R}}_k=\left[\begin{array}{cc}\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}\mathrm{I}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& \frac{{\mathsf{\sigma }}_{m^{\prime }}^2}{{‖{\mathit{m}}_k‖}^2{\left(\sin ({\theta }_{gm})\right)}^2}\mathrm{I}\end{array}\right]$$

(14)

By combining Equations (5), (11), (12) and (14), the EKF approach can then be used for the estimation of both the inclination and the azimuth [12,13,14,15].

3. Simulation, Experimental Results, and Discussion

Depending on the newly formulated measurement model and its error covariance matrix, shown in Equations (11) and (14), respectively, our creatively modified EKF now has the ability to solve the decoupling problem, that is, eliminating the negative effect of magnetic disturbance on the calculation of the inclination. According to the literature, the extended complementary filter (ECF) [40], the hybrid four-parameter complementary filter (FPCF) [41], and the fast complementary filter (FCF) [42] algorithms have the ability to prevent the magnetometer’s measurements from affecting the calculation of the inclination. In this section, the proposed EKF configuration will be compared with these three algorithms in order to assess the superiority of the former approach through the implementation of a thorough analysis.

3.1. Static Evaluation

In this test, the sensor outputs were given as follows:

$$\left\{\begin{array}{l}{\mathit{\omega }}_k={\mathit{\epsilon }}_k^{\omega }\hfill \\ {\mathit{g}}_k={\mathit{\epsilon }}_k^g\hfill \\ {\mathit{m}}_k={\mathit{\epsilon }}_k^m\hfill \end{array}\right.$$

(15)

where the covariance matrices ${\Sigma }_{\omega }$, ${\Sigma }_{\mathit{g}}$, and ${\Sigma }_m$ were set to the values of ${0.006}^2\mathbf{I}$, ${0.008}^2\mathbf{I}$, and ${0.001}^2\mathbf{I}$, respectively. Moreover, the sampling time period $T_s$ was set to $1/100 \mathrm{s}$, $‖{\mathit{g}}_k^0‖=9{.8 \mathrm{m}/\mathrm{s}}^2$, and $‖{\mathit{m}}_k^0‖=0.5 \mathrm{Gs}\left(\mathrm{Gauss}\right)$, whereas the true angle between ${\mathit{g}}_k^0$ and ${\mathit{m}}_k^0$ was ${\theta }_{gm}^0=32°$, and the initial rotation matrix at time instant $k{T}_s=0$ was estimated using the following expression:

$$\mathit{C}({\mathit{q}}_0)=\left[\begin{array}{ccc}0.9953& -0.0772& -0.0578\\ 0.0543& 0.9441& -0.3252\\ 0.0797& 0.3206& 0.9439\end{array}\right]$$

(16)

The filter gains of both the ECF and FPCF are the only parameters that need to be optimally determined. Figure 1 depicts the distribution of the standard deviations of the estimation errors of the three Euler angles in conjunction with the changes in the filter gains $K_{\mathrm{norm}}$ and a of the ECF and FPCF, respectively. Moreover,

Table 1. Minimal values of standard deviations of the estimation errors of the three Euler angles that were estimated using the EKF, ECF, and FPCF approaches.

	EKF	ECF	FPCF
pitch	0.0129	0.0134 (Knorm = 7.3)	0.0134 (a = 7.4)
roll	0.0130	0.0134 (Knorm = 3.9)	0.0134 (a = 7.2)
azimuth	0.0213	0.0210 (Knorm = 2.4)	0.0383 (a = 6.6)

summarizes the minimal values of these standard deviations and their corresponding filter gains, which were obtained by implementing the ECF and FPCF algorithms. By setting the noise parameters of the EKF to ${\mathsf{\sigma }}_{\omega }^2={0.006}^2$, $\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}=\frac{{0.008}^2}{{9.8}^2}$, and $\frac{{\mathsf{\sigma }}_{m^{\prime }}^2}{{‖{\mathit{m}}_k‖}^2{\left(\sin ({\theta }_{gm})\right)}^2}=1.89\times \frac{{0.001}^2}{{0.5}^2}$, the standard deviations provided by the EKF were calculated, as summarized in Table 1. As can be ascertained, although the static performances of both the ECF and FPCF were promising, they did not approach the respective outcomes of the EKF.

3.2. Rotational Test by Incorporating Magnetic Disturbance

In this evaluation process, the sensor outputs were given as follows:

$$\left\{\begin{array}{l}{\mathit{\omega }}_k=\left\{\begin{array}{cc}{\mathit{\epsilon }}_k^{\omega }\hfill & \hfill \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}1\le k<k_1 \mathrm{or} k_2<k\le k_N\\ \left[\begin{array}{c}{\mathsf{\alpha }}_x^{\mathit{\omega }}\sin [{\beta }_x^{\mathit{\omega }}(k-k_1)\pi T_s]\\ {\mathsf{\alpha }}_y^{\mathit{\omega }}\sin [{\beta }_y^{\mathit{\omega }}(k-k_1)\pi T_s]\\ {\mathsf{\alpha }}_z^{\mathit{\omega }}\sin [{\beta }_z^{\mathit{\omega }}(k-k_1)\pi T_s]\end{array}\right]+{\mathit{\epsilon }}_k^{\mathit{\omega }}\hfill & \hfill \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{k}_1\le k\le k_2\end{array}\right.\hfill \\ {\mathit{g}}_k={\mathit{\epsilon }}_k^g\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}1\le k\le k_N\hfill \\ {\mathit{m}}_k=\left\{\begin{array}{ll}{\mathit{\epsilon }}_k^m\hfill & \hfill 1\le k<k_3 \mathrm{or} k_4<k\le k_N\\ {\mathit{\mu }}_k^m+{\mathit{\epsilon }}_k^m=\left[\begin{array}{c}{\mathsf{\alpha }}_x^m\sin [{\beta }_x^m(k-k_3)\pi T_s]\\ {\mathsf{\alpha }}_y^m\sin [{\beta }_y^m(k-k_3)\pi T_s]\\ {\mathsf{\alpha }}_z^m\sin [{\beta }_z^m(k-k_3)\pi T_s]\end{array}\right]+{\mathit{\epsilon }}_k^m\hfill & \hfill k_3\le k\le k_4\end{array}\right.\hfill \end{array}\right.$$

(17)

In Equation (17), $k_1=500$, $k_2=2500$, $k_3=1000$, $k_4=3500$, $k_N=4000$, ${\beta }_x^{\omega }=3$, ${\beta }_y^{\omega }=4$, ${\beta }_z^{\omega }=5$, ${\beta }_x^m=1$, ${\beta }_y^m=2$, and ${\beta }_z^m=3$. The test was executed 2000 times, and in each run, ${\mathsf{\alpha }}_i^{\omega }(i=x,y,z)$ and ${\mathsf{\alpha }}_i^m(i=x,y,z)$ were randomly selected from the uniform distributions bounded by [0, 3] and [0, 0.5], respectively. The initial rotation matrix at a time instant of $k{T}_s=0$ was derived using Equation (16). Additionally, the filter gains of both the ECF and FPCF were set to $K_{norm}=2.4$ and $a=6.6$.

Figure 2 shows the extracted curves of the average absolute error of the inclination and azimuth estimates, which is defined as follows:

$${\mathsf{\xi }}_u(k)=\frac{1}{2000}{\displaystyle \sum _{\mathbf{I}=1}^{2000}\left|u_{est}(k)-u_{tru}(k)\right|}\hspace{1em}\hspace{1em}1\le k\le k_N$$

(18)

where the letter “u” denotes the pitch, the roll, or the azimuth angle, $u_{est}$ signifies the estimate of the given Euler angle, and $u_{tru}$ represents the true value of this Euler angle.

From the calculated results presented in Figure 2, we can draw the conclusion that the inclination estimation accuracy achieved using both the ECF and FPCF approaches can still be affected by magnetic disturbance. Moreover, a deeper insight into this figure reveals that the magnetic disturbance effect may have a relationship with the rotational movement, since it can be clearly observed that magnetic-disturbance-induced accuracy degradation only occurred when a rotational movement took place (see the curves in the time interval $1000\le k\le 2500$). Additionally, in Figure 2, we can observe that the degradation of the inclination and azimuth estimation accuracy of the FPCF was also induced due to the impact of the rotational movements (see the curves in the time interval $500\le k\le 1000$).

In order to further explore the relationship between the inclination estimation accuracy, angular velocity, and magnetic disturbance, we employed Equation (17) in the following parts of this section by setting ${\mathsf{\alpha }}_x^{\omega }={\mathsf{\alpha }}_y^{\omega }={\mathsf{\alpha }}_z^{\omega }$ and ${\mathsf{\alpha }}_x^m={\mathsf{\alpha }}_y^m={\mathsf{\alpha }}_z^m$. The derived outcomes are shown in Figure 3.

As far as the ECF is concerned, it was found that the magnetic disturbance had a slight impact on its inclination estimation accuracy when the angular velocity was low, while the disturbance almost linearly decreased its inclination estimation accuracy when the angular velocity was high. This outcome implies that it may not be suitable to compensate the measurement error of the gyroscope by directly using the measurements of both the accelerometer and magnetometer. In other words, the normal practice, i.e., employing the measurements of both the accelerometer and the magnetometer in order to compensate for the prediction error of the inclination and azimuth calculated in the measurements of the gyroscope, may still be a better choice. As a result, different from the ECF approach, it is finally revealed that the rotational movement—not the magnetic disturbance—causes the degradation of the inclination estimation accuracy of the FPCF. Thus, as expected, magnetic disturbance does not affect the inclination estimation accuracy of the FPCF.

The authors of the FPCF method proposed a new inclination and azimuth representation, where 4D parameters (a 3D unit vector and one angle quantity) are used. However, the equation regarding the derivative of this new representation is nonlinear. Hence, in order to simply discretize the above-mentioned derivative equation, the authors used the first-order difference quotient to approximate the derivative equation, which explains why the inclination and azimuth estimations of the FPCF are susceptible to rotational movements.

3.3. Performance of FCF

The FCF is actually a two-step complementary filter, which fuses the measurements of the magnetometer in the second step for the update of the azimuth estimate. A comparative advantage of this algorithm is that, once a magnetic disturbance is recognized, the second step is not executed, and as a result, the influence of magnetic disturbance on the FCF can be suppressed.

Equation (19) is used in the FCF in order to determine whether a magnetic disturbance exists:

$$\left|\left|{\mathit{m}}_k\right|-\left|{\mathit{m}}_k^0\right|\right|\ge {\mathsf{\alpha }}_m$$

(19)

The authors of the FCF did not comment on the exact value of ${\mathsf{\alpha }}_m$ in their work; therefore, we set ${\mathsf{\alpha }}_m=3{\mathsf{\sigma }}_m$, which is based on the PauTa criterion. The filter gain of the FCF was set to ${\gamma }_a={\gamma }_m=0.1$.

We still employed Equation (17) in this section by setting ${\mathsf{\alpha }}_x^{\omega }={\mathsf{\alpha }}_y^{\omega }={\mathsf{\alpha }}_z^{\omega }=3$ and ${\mathsf{\alpha }}_x^m={\mathsf{\alpha }}_y^m={\mathsf{\alpha }}_z^m=0.5$. Figure 4 illustrates the change curves of ${\mathsf{\xi }}_u$ for the inclination and azimuth estimates. It is apparent that the impact of the magnetic disturbance on the FCF significantly depends on the judgment accuracy due to the existence of the magnetic disturbance. On top of that, if the movements are complicated, then it is inevitable that the magnetic disturbance could affect the inclination estimation accuracy due to the inaccurate judgment of its existence, as shown in Figure 4.

3.4. Performance Evaluation Using Real Sensor Measurements

In this test, an MIMU named MTi-G, which was created by Xsens (Henderson, NV, USA), was placed on a wooden table and kept motionless. After a while, a mobile phone, which was used in order to introduce magnetic disturbance, was moved around the MTi-G with the assistance of the tester’s hand. The standard deviations of the measurement errors of the gyroscope, accelerometer, and magnetometer were $0.0065 \mathrm{rad}/\mathrm{s}$, $0{.01 \mathrm{m}/\mathrm{s}}^2$, and $0.002 \mathrm{Gs}$, respectively. In addition, the sampling rate of the MTi-G was 10 Hz. Figure 5 depicts the inclination and azimuth estimation results provided by the EKF, ECF, and FPCF. The filter gains of the three algorithms were all optimally determined using the method explained in part A of this section. More specifically, they were set to the following values: ${\mathsf{\sigma }}_{\omega }^2={0.0065}^2$, $\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}=\frac{{0.01}^2}{{9.8}^2}$, $\frac{{\mathsf{\sigma }}_{m^{\prime }}^2}{{‖{\mathit{m}}_k‖}^2{\left(\sin ({\theta }_{gm})\right)}^2}=10\times \frac{{0.002}^2}{{0.8}^2}$, $K_{norm}=3.1$, and $a=4.38$.

Furthermore, the sensor measurements were down-sampled from 1 Hz to 9 Hz, and at each sampling rate, the filter gains of the EKF, ECF, and FPCF were optimally re-determined using the method explained in part A of this section. The standard deviations of the inclination estimation errors provided by the EKF, ECF, and FPCF at each sampling rate are highlighted in Figure 6. The standard deviation was computed as follows:

$${\mathsf{\sigma }}_u=\sqrt{\frac{1}{k_N-1}{\displaystyle \sum _{k=1}^{k_N}{\left|u_{est}(k)-\mathsf{\mu }\right|}^2}}\hspace{1em}\hspace{1em}1\le k\le k_N$$

(20)

where $k_N=778$, and

$$\mathsf{\mu }={\displaystyle \sum _{k=1}^{k_N}{u}_{est}(k)}\hspace{1em}\hspace{1em}1\le k\le k_N$$

(21)

The profiles of the inclination (pitch (upper left) and roll (upper right)) and azimuth (down middle) estimates provided by the EKF, ECF, and FPCF are given in Figure 5. As anticipated, it can be ascertained from Figure 5 that all three algorithms performed well, and only the azimuth estimation accuracy of these algorithms was affected by the magnetic disturbance. However, a deeper insight into the extracted data reveals that the azimuth estimation accuracy of the EKF seemed to be more immune to the magnetic disturbance compared with the other two algorithms.

As observed in Figure 6, after decreasing the sampling rate, the performances of both the ECF and FPCF deteriorated dramatically, while the performance of the EKF was only slightly affected. Hence, we can draw the conclusion that the EKF outperforms both the ECF and FPCF when the sampling rate is low.

Another MIMU, named MTi-300, was rotated almost periodically with the assistance of the tester’s hand. The rotation was performed around a computer case, which can distort the Earth’s magnetic field in the range of the rotation motion. An electromagnetic tracking system created by Polhemus (Colchester, CT, USA) was taken as the attitude reference. The standard deviations of the measurement errors of the gyroscope, accelerometer, and magnetometer were $0.002 \mathrm{rad}/\mathrm{s}$, $0{.008 \mathrm{m}/\mathrm{s}}^2$, and $0.001 \mathrm{Gs}$, respectively. In addition, the sampling rate of the MTi-300 was 100 Hz. Figure 7 depicts the inclination and azimuth estimation results provided by the EKF, ECF, and FPCF. The filter gains of the three algorithms were all optimally determined using the method explained in part A of this section. More specifically, they were set to the following values: ${\mathsf{\sigma }}_{\omega }^2={0.002}^2$, $\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}=\frac{{0.008}^2}{{9.8}^2}$, $\frac{{\mathsf{\sigma }}_{m^{\prime }}^2}{{‖{\mathit{m}}_k‖}^2{\left(\sin ({\theta }_{gm})\right)}^2}=26\times \frac{{0.001}^2}{{0.8}^2}$, $K_{norm}=1.6$, and $a=7.1$.

Since linear disturbance is introduced during the rotation motion, the filter gains should be dynamically regulated for linear disturbance suppression, as follows.

$$\left\{\begin{array}{l}{\mathsf{\sigma }}_{\omega }^2=\frac{1}{\mathsf{\mu }}{\mathsf{\sigma }}_{\omega }^2\hfill \\ \frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}=\mathsf{\mu }\frac{{\mathsf{\sigma }}_g^2}{{‖{\mathit{g}}_k‖}^2}\hfill \\ \frac{{\mathsf{\sigma }}_{m^{\prime }}^2}{{‖{\mathit{m}}_k‖}^2{\left(\sin ({\theta }_{gm})\right)}^2}=\mathsf{\mu }\frac{{\mathsf{\sigma }}_{m^{\prime }}^2}{{‖{\mathit{m}}_k‖}^2{\left(\sin ({\theta }_{gm})\right)}^2}\hfill \\ K_{norm}=\mathsf{\mu }{K}_{norm}\hfill \\ a=\frac{1}{\mathsf{\mu }}a\hfill \end{array}\right.$$

(22)

The scalar $\mathsf{\mu }$ is determined according to the following:

$$\mathsf{\mu }=\left\{\begin{array}{ll}1,\hfill & \hfill \left|\left|{\displaystyle \sum _{i=k-N+1}^k{\mathit{g}}_i}\right|-\left|{\mathit{g}}_k^0\right|\right|\le 3\sqrt{3}{\mathsf{\sigma }}_g\\ {10}^{100},\hfill & \hfill \left|\left|{\displaystyle \sum _{i=k-N+1}^k{\mathit{g}}_i}\right|-\left|{\mathit{g}}_k^0\right|\right|>3\sqrt{3}{\mathsf{\sigma }}_g\end{array}\right.$$

(23)

where $N=20$.

Obviously, the negative effect caused by the linear acceleration disturbance is suppressed, but the negative effect caused by the magnetic disturbance is completely remains. The profiles of the inclination (pitch (upper left) and roll (upper right)) and azimuth (down middle) estimates provided by the EKF, ECF, and FPCF are given in Figure 7.

As anticipated, it can be ascertained from Figure 7 that the EKF behaves basically the same as the ECF, except that the dynamic response of the EKF is a bit faster than that of the ECF. However, the azimuth estimation accuracy of the FPCF is significantly decreased. The FPCF heavily relies on the gyroscope measurements for attitude estimation when Equation (22) is employed. As mentioned above, the authors of the FPCF used the first-order difference quotient to approximate the derivative equation, which may explain the poor azimuth estimation performance. In addition, the magnetic disturbance further deteriorates the azimuth estimation performance. It is noted that the orientation (including the pitch, roll, and azimuth) estimation errors shown in Figure 7 are mostly caused by the gyroscope measurement error and the comparatively low sampling rate, and the azimuth estimation error is partly caused by the magnetic disturbance. Due to the similarity between the pitch and roll estimation accuracy, it is deduced that the magnetic disturbance does not negatively affect the pitch and roll calculation of the EKF, ECF, and FPCF.

4. Conclusions

In this work, we have demonstrated that the EKF is an important alternative solution in applications where the influence of magnetic disturbance on the inclination estimation is not acceptable. Despite the fact that three algorithms, i.e., ECF, FPCF, and FCF, have recently been proposed, there are still significant issues that should be addressed. The ECF is susceptible to magnetic disturbance when a large angular movement takes place. Moreover, the FPCF cannot perform well when only a large angular movement exists, regardless of whether a magnetic disturbance exists or not. Additionally, both the ECF and FPCF exhibit poor performance when the sampling rate is decreased. The impact of magnetic disturbance on the FCF strongly depends on the identification accuracy for the existence of magnetic disturbance. With some modifications, the EKF not only achieves the goal of removing the influence of the magnetometer’s measurements on the inclination determination, but also overcomes the above-mentioned issues faced by the ECF, FPCF, and FCF algorithms. Hence, it is concluded that the EKF can be selected for the related applications described in our work, and is still the preferred algorithm for determining the attitude of rigid bodies.