A Dual-Linear Kalman Filter for Real-Time Orientation Determination System Using Low-Cost MEMS Sensors

Abstract

To provide a long-time reliable orientation, sensor fusion technologies are widely used to integrate available inertial sensors for the low-cost orientation estimation. In this paper, a novel dual-linear Kalman filter was designed for a multi-sensor system integrating MEMS gyros, an accelerometer, and a magnetometer. The proposed filter precludes the impacts of magnetic disturbances on the pitch and roll which the heading is subjected to. The filter can achieve robust orientation estimation for different statistical models of the sensors. The root mean square errors (RMSE) of the estimated attitude angles are reduced by 30.6% under magnetic disturbances. Owing to the reduction of system complexity achieved by smaller matrix operations, the mean total time consumption is reduced by 23.8%. Meanwhile, the separated filter offers greater flexibility for the system configuration, as it is possible to switch on or off the second stage filter to include or exclude the magnetometer compensation for the heading. Online experiments were performed on the homemade miniature orientation determination system (MODS) with the turntable. The average RMSE of estimated orientation are less than 0.4° and 1° during the static and low-dynamic tests, respectively. More realistic tests on two-wheel self-balancing vehicle driving and indoor pedestrian walking were carried out to evaluate the performance of the designed MODS when high accelerations and angular rates were introduced. Test results demonstrate that the MODS is applicable for the orientation estimation under various dynamic conditions. This paper provides a feasible alternative for low-cost orientation determination.

1. Introduction

Accurate orientation is essential for the locating and tracking of moving objects relative to a given frame in various kinds of applications, such as unmanned aerial vehicle (UAV) navigation [1], autonomous underwater vehicle (AUV) [2], self-driving cars [3], intelligent robots [4], wearable devices [5], and human position tracking [6,7,8] etc., in military and industrial areas. Inertial sensors, such as the gyros and accelerometer make it possible to determine the orientation of moving object by measuring kinetic physical quantities (acceleration and angular rate, etc.) without any external additional information and other restrictions [9]. Gimbaled gyros, fiber optic gyros, and laser gyros have been successfully used to provide the high-precision movement information for the aircraft and missile systems in military and aviation industries. Given an initial assumption of the orientation, gyros can provide accurate orientation information via integrating the angular rates of the moving object numerically [10]. However, high-performance gyros are usually very expensive and bulky. Even the access to these devices may be limited. With the rapid expansions of the civilian and consumer markets, more and more micro-electro-mechanical system (MEMS)-based gyros are used for the low-precision orientation determination, benefited from their low price, small size, low-power dissipation, and relatively high reliability [11].

However, due to the inherent noise and drifts with time, there are considerable cumulative errors when only the gyros are used for the orientation determination. Various kinds of time series models are used to estimate the stochastic process of the gyro [12]. All of the published literature shows that the cumulative errors cannot be eliminated only according to the stochastic model of the gyro. Even for the extremely expensive MEMS gyros, the cumulative errors still cannot be neglected. Accordingly, it is necessary to integrate the gyro with some other sensors which there are no drifts and cumulative errors existing. Yun, et al. proposed a factored quaternion method for the orientation estimation [13]. Derivations of half-angle formulas were proposed in their paper. Taking advantage of the gravity decomposition, attitude angles (namely, the pitch and roll angles) are easily obtained from the accelerometer. Sequentially, the heading angle is available when the magnetometer is used. However, the gravity is coupled with the kinematic accelerations so strongly that it cannot be separated from the accelerometer outputs accurately, especially in the case of long-term high dynamics. Simultaneously, the geomagnetic field is very susceptible to the additional magnetic field induced by surrounding hard or soft magnet materials [14]. Due to the deviations among the directions of the geomagnetic field in different locations, it is not appropriate to use the magnetic data for the pitch and roll estimation. All the reasons above make the accelerometer and magnetometer play the role of aided sensors.

Nowadays, typical MEMS-based orientation systems usually consist of three single-axis gyros and an electronic compass which includes a tri-axis accelerometer and a tri-axis magnetometer [15,16,17]. It is crucial to develop appropriative embedded data fusion solutions for the orientation systems. The Kalman filter (KF) has already become the most commonly used sensor fusion technique for MEMS orientation systems. Various filter models were developed for different orientation representations, such as the direction cosine matrix, Euler angles, and quaternion. To simplify the implementation of KF, Zhu, et al. developed a linear KF in which the state vector was composed of the gravity and geomagnetic field in body frame [18]. A linear Kalman model was designed and the effect of the forgetting factor on the time lag was also investigated in their paper. Han and Wang proposed an Euler angle errors-based method to express errors in the local level frame rather than the body frame so as to achieve a linear KF [19]. Though the nonlinearity problem was avoided, the observation model was still faced with the singularity problem in their approach when the pitch angle gets through the area near ±π/2. Li and Wang developed an improved linear KF based on the psi-angle propagation equation [20]. The residuals of heading angle and accelerations were defined as the observation vector. In their paper, an adaptive gain is tuned for the KF according to the dynamic scale determined by the accelerometers when the system is in a high dynamic mode. However, not only the heading, but also that of the pitch and roll, would be affected if the magnetometer measurements were used with the accelerometer measurements directly. Furthermore, Sabatelli, et al. designed a two-stage extended Kalman filter (EKF) to calculate the attitude angles and the heading angle separately [21]. The accelerometer was used for the attitude determination in the first stage filter and the magnetometer was used for the heading correction in the second one. Due to the nonlinearity of the measurement equations in their method, high-order matrix operations lead to considerable increases in iterative computations eventually. Instead of implementing EKF, we designed an unscented Kalman filter (UKF) to obtain the high-accuracy indoor heading estimation in our previous works [22]. The UKF was deemed to be more accurate and less computationally costly than EKF, while too many trigonometric functions and Taylor expansions greatly increased the complexity of system. What is worse, the singularity problem was unavoidable in the UKF-based method.

In this paper, we focused on the requirements of a complete system for the low-cost orientation determination. A novel dual-linear Kalman filter was designed for the multi-sensor system. The kinematic models were based on the propagation equations of the local gravity and geomagnetic field in the body frame. The outputs of accelerometer and magnetometer were defined as the measured quantities in the two independent observation models. Benefited from the specific design, only the gyro and accelerometer are enough to run the first stage filter for the attitude estimation and a magnetometer could be integrated optionally if the heading is needed. Considering the different statistical models for sensor errors, the proposed filter can achieve optimal orientation estimation if the sensor statistical error is assumed to be white noise. The proposed strategy precludes the impacts of geomagnetic distortions on the pitch and roll which the heading is subjected to. The RMSE of attitude angles are reduced by 30.6% under magnetic disturbances. Meanwhile, owing to the reduction of system complexity, total time consumption of the proposed method is reduced by 23.8% than that of a standard one, which means that a higher frequency can be implemented for the update of orientation. Furthermore, the separated filter design offers greater flexibility and robustness to magnetic anomalies for the system configuration, as it is possible to switch on or off the second filter to include or exclude the magnetometer compensation for the heading. Online experiments were performed on the homemade real-time miniature orientation determination system (MODS). Furthermore, we carried out more realistic tests on the two-wheel self-balancing vehicle driving and indoor pedestrian walking to evaluate the performance of the designed multi-sensor system when high linear accelerations and angular rates were introduced. The test results demonstrated that the accuracy and stability of MODS were well guaranteed. This paper provides a feasible alternative for low-cost orientation determination.

The kinematic modeling is introduced for the multi-sensor system in Section 2. The dual-linear filter is proposed in Section 3. Noise characteristics are analyzed for the sensor models in Section 4. Hardware design is described for the MODS in Section 5. Real-time experiments performed on the homemade MODS are presented and discussed in Section 6. Finally, the forecasts are put forward for further study in Section 7.

2. System Modeling

The orientation system is usually applied to determine Euler angles which are regarded as the essential parameters for the navigation and motion control. So-called Euler angles are defined as the rotation angles from the given inertial frame (usually called the navigation frame, denoted by n) to the body frame (fixed to the moving object and denoted by b), including yaw $\psi$, pitch $\theta$, and roll $\varphi$. The body frame $xyz$ is attached to the orientation system. The $x$-axis is aligned with its forward direction, the $z$-axis points to the bottom of the system and the $y$-axis rounds up the right-handed orthogonal coordinate frame. In this paper, the navigation frame n is attached to the local level north frame, namely the North, East, Down (NED) frame, as shown in Figure 1.

Here, we designate a column vector $\overrightarrow{u}$, whose components are generally functions of the time $t$. The transformation between the vector $\overrightarrow{u}$ in the frame b and the frame n is as:

$${\overrightarrow{u}}^n\left(t\right)=C_b^n\left(\varphi ,\theta ,\psi \right){\overrightarrow{u}}^b\left(t\right)$$

(1)

where $C_b^n\left(\varphi ,\theta ,\psi \right)$ represents the direction cosine matrix (DCM) used to transform the measured quantities from the frame b into the frame n. Henceforward, the parameter t will be omitted for the convenience of readers. $C_b^n$ can be carried out through three different separate rotations about the three axes. $C_{\psi }^z$, $C_{\theta }^y$, and $C_{\varphi }^x$ represent the rotation $\psi$ angle about the $z$-axis, $\theta$ angle about the $y$-axis, and $\varphi$ angle about the $x$-axis, respectively, which are defined as:

$$C_{\psi }^z=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right],\begin{array}{c}{C}_{\theta }^y=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\end{array},\begin{array}{c}{C}_{\varphi }^x=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi & \cos \varphi \end{array}\right]\end{array}$$

(2)

Then:

$$C_b^n=C_{\psi }^z{C}_{\theta }^y{C}_{\varphi }^x=\left[\begin{array}{ccc}\cos \psi \cos \theta & \cos \psi \sin \theta \sin \varphi -\sin \psi \cos \varphi & \cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi \\ \sin \psi \cos \theta & \sin \psi \sin \theta \sin \varphi +\cos \psi \cos \varphi & \sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi \\ -\sin \theta & \cos \theta \sin \varphi & \cos \theta \cos \varphi \end{array}\right]$$

(3)

The propagation equation of $C_b^n$ accords with the following equation in [23]:

$${\dot{C}}_b^n=C_b^n\cdot \left[\begin{array}{ccc}0& -r& q\\ r& 0& -p\\ -q& p& 0\end{array}\right]=C_b^n\cdot {\Omega }_{nb}^b$$

(4)

where ${\Omega }_{nb}^b$ is the skew symmetric matrix of $\omega ={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^T$, which is the angular rate of the moving object in the frame b. Differentiating Equation (1) with respect to the time:

$${\dot{\overrightarrow{u}}}^n={\dot{C}}_b^n\cdot {\overrightarrow{u}}^b+C_b^n\cdot {\dot{\overrightarrow{u}}}^b$$

(5)

Then, the propagation equation of $\overrightarrow{u}$ in the body frame can be derived as:

$${\dot{\overrightarrow{u}}}^b=-{\Omega }_{nb}^b\cdot {\overrightarrow{u}}^b$$

(6)

The vectors of the gravity and the geomagnetic field will both yield to Equation (6):

$$\{\begin{array}{l}{\dot{\overrightarrow{g}}}^b=-{\Omega }_{nb}^b\cdot {\overrightarrow{g}}^b\\ {\dot{\overrightarrow{m}}}^b=-{\Omega }_{nb}^b\cdot {\overrightarrow{m}}^b\end{array}$$

(7)

where ${\overrightarrow{g}}^b={\left[\begin{array}{ccc}{g}_x^b& g_y^b& g_z^b\end{array}\right]}^T$ and ${\overrightarrow{m}}^b={\left[\begin{array}{ccc}{m}_x^b& m_y^b& m_z^b\end{array}\right]}^T$ represent the vectors of the gravity and the geomagnetic field in the frame b, respectively.

3. Dual-Linear Kalman Filter Design

As discussed in the introduction, the orientation angles obtained from the accelerometer and magnetometer provide long-term accuracy with high noise while the gyro-derived orientation angles suffer from drifts and cumulative errors. Neither of them will achieve accurate and stable orientation when only one method is used alone. Therefore, a dual-linear Kalman filter is designed to integrate these two approaches together for accurate orientation estimation in this paper. Unlike Zhu et al. [18], defining the system state vector as $S={\left[\begin{array}{cc}{\overrightarrow{m}}^b& {\overrightarrow{g}}^b\end{array}\right]}^T$, we define two state vectors which are the gravity and geomagnetic field in the body frame b in this paper, respectively:

$${\overrightarrow{x}}_1={\overrightarrow{g}}^b,{\overrightarrow{x}}_2={\overrightarrow{m}}^b$$

(8)

Similarly, the accelerometer and magnetometer outputs are defined as the two observation vectors corresponding to the state vectors:

$${\overrightarrow{z}}_1=\overrightarrow{a},{\overrightarrow{z}}_2=\overrightarrow{m}$$

(9)

The unified discrete-time dynamic equation of the system state can be expressed as follows:

$$\begin{array}{l}{\overrightarrow{x}}^n=\Phi {\overrightarrow{x}}^{n-1}+{\overrightarrow{w}}^{n-1}\\ {\overrightarrow{z}}^n={\overrightarrow{x}}^n+{\overrightarrow{v}}^n\end{array}$$

(10)

where ${\overrightarrow{w}}^n$ and ${\overrightarrow{v}}^n$ are the process and measurement noises vectors, respectively. The system transfer matrix $\Phi$ is given as:

$$\Phi =\text{exp}\left(-{\Omega }_{nb}^b\cdot t_s\right)$$

(11)

where $t_s$ is the sampling interval. After estimating of the state vectors, the pitch, roll, and heading are obtained by the following arc-tangent formulas:

$$\{\begin{array}{l}\theta =-\text{arctan}(\frac{g_x^b}{\sqrt{{\left(g_y^b\right)}^2+{\left(g_z^b\right)}^2}}),\varphi =\text{arctan}(\frac{g_y^b}{g_z^b})\\ \psi =\text{arctan}(\frac{-m_y^b\cos \varphi +m_z^b\sin \varphi }{m_x^b\cos \theta +m_y^b\sin \varphi \sin \theta +m_z^b\cos \varphi \sin \theta })\end{array}$$

(12)

The next task is to compute the Kalman gain which is defined as:

$$K_n=P_n^{-}{H}^T{\left({\rm H}{P}_n^{-}{{\rm H}}^T+R_n\right)}^{-1}$$

(13)

where $R_n$ is the covariance matrix of measurement noise ($R_a$ for the accelerometer and $R_m$ for the magnetometer). As the inverse of the 3 × 3 order matrix ${\rm H}{P}_n^{-}{{\rm H}}^T+R_n$ is formulized, time consumption will be drastically reduced.

When the orientation system is in stable states, it is easy to achieve the optimal $R_n$. However, inertial sensor measurements may be unreliable and even useless in dynamic states. An adaptive mechanism is designed for the covariance matrixes of sensors noise in the filtering processes according to [20,24]. In the absence of magnetic disturbances, the locus of magnetometer output lies on the surface of a sphere. However, deviations of the magnetometer measurements are very large if magnetic disturbances exists. There are abundant literatures regarding the magnetometer calibration [25,26,27]. The method proposed in [28] was executed for the magnetometer calibration. The strategy will be performed for $R_m$ as follows:

$$R_m=\{\begin{array}{l}{\sigma }_m^{2}I,\text{if}\left|\Vert {\overrightarrow{m}}_{i+1}\Vert -\Vert \overrightarrow{h}\Vert \right|<{\xi }_m\\ \infty ,\text{otherwise}\end{array}$$

(14)

The schematic of the proposed dual-linear filter algorithm is shown in Figure 2. The state vectors are divided into two independent linear filters and updated separately.

4. Noise Characteristics

The covariances of the process noise and measurement noise are regarded as the primary design parameters to achieve the minimum variance of estimation errors. In this paper, the process noise is mainly derived from the angular rates measured by the gyros. We can assume that there is a small perturbation $\delta \omega =\left[\begin{array}{ccc}\delta p& \delta q& \delta r\end{array}\right]$ to $\omega$:

$$\omega =\overline{\omega }+\delta \omega$$

(15)

where $\overline{\omega }=\left[\begin{array}{ccc}\overline{p}& \overline{q}& \overline{r}\end{array}\right]$ is the mean of $\omega$. The state equation can be rewritten as:

$$\begin{array}{l}\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\\ {\dot{x}}_3\end{array}\right]=-\left[\begin{array}{ccc}0& -(\overline{r}+\delta r)& (\overline{q}+\delta q)\\ (\overline{r}+\delta r)& 0& -(\overline{p}+\delta p)\\ -(\overline{q}+\delta q)& (\overline{p}+\delta p)& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\\ =-\left[\begin{array}{ccc}0& -\overline{r}& \overline{q}\\ \overline{r}& 0& -\overline{p}\\ -\overline{q}& \overline{p}& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\underset{\overrightarrow{w}}{\underbrace{-\left[\begin{array}{ccc}0& x_3& -x_2\\ -x_3& 0& x_1\\ x_2& -x_1& 0\end{array}\right]\left[\begin{array}{c}\delta p\\ \delta q\\ \delta r\end{array}\right]}}\end{array}$$

(16)

The second term on the right side of the Equation (16) is regarded as the process noise $\overrightarrow{w}$. For a discrete-time system, the covariance matrix of the process noise $\overrightarrow{w}$ is obtained:

$$Q=L\cdot R_g\cdot L^T$$

(17)

where

$$R_g=diag\left[\begin{array}{ccc}{\sigma }_p^2& {\sigma }_q^2& {\sigma }_r^2\end{array}\right],L=\left[\begin{array}{ccc}0& x_3& -x_2\\ -x_3& 0& x_1\\ x_2& -x_1& 0\end{array}\right]$$

(18)

where ${\sigma }_p^2$, ${\sigma }_q^2$ and ${\sigma }_r^2$ are the variances of $p$, $q$, and $r$.

The performance of the proposed filter is strongly linked with the quality of stochastic models used to describe the different sensors’ noise. In this paper, the sensors’ noise is assumed to be a white noise according to pertinent literatures. A generalized method of wavelet moments (GMWM) proposed by Guerrier et al. [12] was used to analyze the impacts of different sensor error models on the filter. A numerical simulation was implemented to evaluate three different models: (1) a white noise, (2) a Gauss-Markov (GM) process, and (3) a sum of three GM processes. Firstly, we created a theoretical trajectory as shown in Figure 3. The attitude angle is expressed as follows:

$$\theta =\frac{\pi }{2}\sin (0.2t)$$

(19)

where t is the simulation time at a sampling instant.

The “perfect” inertial observations (angular rates and accelerations) were obtained by inverse strapdown. Then, we corrupted these perfect observations by three types of errors generated from the three different sensor models. Finally, the proposed filter was executed with the three kind of corrupted observations. Figure 4 shows the time-varying pitch estimation error based on three different sensor models. Figure 5 shows performance comparisons of the absolute pitch estimation error. As shown in the plots, the proposed filter can achieve robust orientation estimation for different sensors errors models. It can be seen that the proposed filter provides the optimal estimation when the sensors’ noise is assumed as a white noise. Meanwhile, the results of the Gauss-Markov model are very close to the white noise model.

5. Hardware Design

In order to verify and implement the proposed filter in practice, we developed a homemade prototype which is a real-time miniature multi-sensor system, as shown in Figure 6. The multi-sensor system is comprised of three single-axis MEMS gyros (full-scale range of ±450°/s), a tri-axis accelerometer (full-scale range of ±5 g) produced by Analog Devices, and a tri-axis magnetometer (full-scale range of ±0.8 Gauss) produced by Honeywell. Considering the electromagnetic interference, the magnetometer was welded on the back of the PCB. The sensor data fusion processes are handled by the microprocessor of the STM32F4 series for its excellent performance on dealing with large numbers of floating point arithmetic. The raw sensors data and orientation angles are collected from the miniature orientation determination system (MODS) when it is connected to the upper computer via USB or Zigbee.

6. Experiments and Results

6.1. Noise Variances Determination

Firstly, we removed the mean of the original gyro signal produced in static tests at a frequency of 100 Hz (448,915 measurements). The time-varying errors are available and are presented in the time domain in Figure 7 (upper panel), together with the Haar wavelet variance (WV) and the corresponding GMWM for this process with 95% confidence intervals (lower panel). Similar results are achieved by the GMWM for a white noise process and a GM process, which validate the previous results in Section 4 again. The WV computed from the gyro signal give an indication for the underlying stochastic processes. The GMWM estimates of the parameters and their corresponding 95% confidence intervals using the WV covariances are summarized in

Table 1. Estimated parameters with associated 95% confidence intervals for three different stochastic models with the gyro signal data.

Model	Parameter	Estimate	IC (0.95)
Model 1	${\sigma }^2$	4.654064e−06	(4.639158e−06; 4.668971e−06)
Model 2	${\sigma }^2$	4.653360e−06	(4.639986e−06; 4.666734e−06)
	$\beta$	5.342647e−02	(5.106282e−02; 5.579013e−02)
Model 3	${\sigma }_1^2$	9.246705e−12	(5.989062e−12; 1.250435e−11)
	${\beta }_1$	9.990636e−01	(9.990636e−01; 9.990636e−01)
	${\sigma }_2^2$	8.794771e−13	(4.427838e−13; 1.316170e−12)
	${\beta }_2$	9.999861e−01	(9.999861e−01; 9.999861e−01)
	${\sigma }_3^2$	4.658634e−06	(4.640222e−06; 4.677046e−06)
	${\beta }_3$	3.307696e−02	(3.307696e−02; 3.307696e−02)

. The suitability of the estimated models can be judged graphically by a matching of the empirical WV and the parametric WV using the estimates in Table 1.

6.2. Time Consumption Emulation

A time consumption test was designed to validate the effects of proposed filter for reducing the system complexity and computing time by smaller matrix operations. We collected 31,120 row data from an existing orientation system [29] at 100 Hz which were used to run our filter in MATLAB ten times, repeatedly.

Table 2. Comparisons of Method A and Method B.

Method	Mean Time Consumption	Mean Attitude Error
Method A	8.72 s	0.25°
Method B	11.45 s	0.36°

shows the comparisons of time-varying pitch errors estimated by our filter (Method A) and the proposed filter in [18] (Method B). The proposed filter precludes the impacts of geomagnetic distortions on the pitch and roll which the heading is subjected to. As shown in Table 2, the average RMS errors of proposed filter are reduced by 30.6% compared with Method B under strong magnetic disturbances. Meanwhile, mean computation times are reduced by 23.8%, which means that higher frequency can be implemented for the orientation updates.

6.3. MODS Evaluation

Three kinds of experiments were carried out to verify the performance of the designed MODS. Firstly, the static and dynamic tests for MODS were implemented on a tri-axis turntable that allows precise and repeatable tests. Then, the MODS was mounted on a homemade two-wheel self-balancing vehicle for driving control. Finally, the MODS was tied on the shoes of pedestrian for the indoor walking and stair-climbing experiments to validate the robustness on the attitude estimation when high linear accelerations and angular rates were introduced.

6.3.1. Tri-Axis Turntable Experiments for the MODS

As shown in Figure 8, the MODS was fixed on the static turntable relative to the ground during the static tests. In the majority of existing published literatures, orientation systems were usually kept at the zero input point so as to evaluate the static performance. In our paper, different static input points were tested in our static experiments. The attitude angles were tested from −80° to 80° and the heading was from 0° to 90°. The turntable was turned by 10° every time and would be held for a few seconds. During the dynamic test, the MODS was mounted on the running turntable which was controlled to execute the pre-set motions.

As shown in Figure 9, the static test results are very smooth and steady. The dynamic test results are plotted in Figure 10. The proposed filter achieves an excellent performance on orientation estimation, which is free from the drifts of gyros and transient disturbances on the compass successfully.

Overall, the results of MODS are quite consistent with the reference, virtually without any time lag whatsoever in the tests. We can see clearly that gyro-derived orientation angles deviate from the reference due to the inherent biases of the gyros. On the other hand, the orientation angles estimated by the accelerometer and magnetometer tend to follow the reference well in the quasi-static state, but large errors and poor reliability appear in the transient high dynamic state.

Table 3. RMS errors of the MODS in the turntable tests.

Test State	Pitch	Roll	Yaw
Static	0.045°	0.064°	0.352°
Dynamic	0.301°	0.386°	0.845°

indicates that the root mean square (RMS) errors of the pitch and roll angles are less than 0.1°, and approximately 0.4° for the heading angle in static tests. Meanwhile, the RMS errors are below 0.5° for the pitch and roll angles, and less than 1° for the heading angle in dynamic tests.

6.3.2. Experiments on the Two-Wheel Self-Balancing Vehicle Driving

For the purpose to validate the performance of the designed MODS in practical applications, we mounted it on a homemade two-wheel self-balancing vehicle, as shown in Figure 11a. The pitch angle $\theta$ is one of the most important parameters for driving the self-balancing vehicle. The servo motors will adjust the driving torque $T_d$ on the wheels to keep the dynamic balance of the vehicle body according to the variation of the pitch angles, as shown in Figure 11b. At the beginning, the two-wheel vehicle was kept static for a few seconds. Then, the two-wheel vehicle was controlled to drive along the preplanned route until commanded to slow down and stop. Figure 12 shows the time-varying pitch and roll angles during the whole test.

The pitch angle changes rapidly at the time of 11 s and 23 s corresponding to the starting and stopping of the two-wheel vehicle, respectively. The variations of pitch angles are relatively stable during the vehicle running. Meanwhile, the roll angle changes stably within 1° during the whole test.

6.3.3. Indoor Pedestrian Walking and Stair-Climbing Experiments

For the further validation of the accuracy and stability of the MODS in more realistic motions, indoor pedestrian walking and stair-climbing tests were carried out in our college building, as shown in Figure 13. The MODS was tied on the shoes of the pedestrian. During the waking tests, the pedestrian was required to walk straight along the preplanned route on the floor. The floor tiles can be regarded as the natural tags to repeat the walking tests. Raw sensor data and orientation angles were collected from the MODS. As shown in Figure 14, norms of the walking accelerations are much higher than the gravity, even close to the maximum scale range of accelerometer (5 g). Similarly, norms of walking angular rates are close to the maximum scale range of gyros (450°/s). Therefore, high linear accelerations and angular rates are introduced into the proposed filter to verify the robustness of the attitude estimation. The walking cycles are shown clearly by identifiable peak points which can be applied to indoor pedestrian navigation and human health monitoring.

Figure 15 shows the real-time attitude angles of the MODS during walking tests. Similar to the variations of accelerations and angular rates in Figure 14, attitude angles change periodically with the pedestrian waking cycles. The MODS can provide stable enough attitude estimation for the pedestrian walking.

During the stair-climbing tests, the pedestrian was required to climb three floors including six half floors climbing and six corners walking which are corresponding to the red segments and the blue segments of the time-varying curves of the accelerations and angular rates in Figure 16. We can find out that the variation ranges of the climbing accelerations are much larger than that of the walking accelerations, but the variation ranges of the angular rates show an inverse tendency. Therefore, the proposed filter will rely more on the gyro outputs during the pedestrian walking, while the accelerometer outputs are considered more in the quasi-static attitude updates.

Figure 17 shows the real-time attitude angles estimated by the MODS during the pedestrian stair-climbing tests. The pitch and roll angles of the walking have a larger variation range than that of climbing stairs. The results are very consistent with the realistic motions because the pedestrian’s feet can stretch completely when walking while they will be blocked by the steps when climbing stairs. Figure 18 is the close-ups relevant to the pitch and roll angles in Figure 17, which shows an excellent convergence and dynamic performance.

7. Conclusions

In this paper, a novel dual-linear Kalman filter was designed for the orientation determination system using low-cost MEMS-based sensors. The propagation equations of the local gravity and geomagnetic field in frame b are used to establish the dynamic models. The accelerometer and magnetometer outputs are defined as the two measured quantities in the same, but independent, observation models. The proposed strategies precludes the impacts of magnetic disturbances on the pitch and roll angles which the heading is subjected to. The RMSE of estimated attitude angles are reduced by 30.6% compared with a standard filter under magnetic disturbances. The proposed filter can achieve optimal orientation estimation if the sensor statistical error is assumed to be white noise. Owing to the reduction of system complexity, time consumption is reduced by 23.8% compared with a standard filter, which means that higher frequency can be implemented for the orientation updates. The proposed separated method offers greater flexibility for the system design. Online experiments performed on a real-time homemade MODS demonstrate that the proposed sensor fusion algorithm can maintain an accurate and stable estimation for the orientation. The average RMSE are less than 0.4° and 1° in the static and low-dynamic turntable tests, respectively. More realistic tests were carried out on the two-wheel self-balancing vehicle driving and indoor pedestrian walking to evaluate the performance of the MODS. The results demonstrate that the accuracy and stability of the MODS are guaranteed even with high linear accelerations and angular rates, which shows remarkable robustness over the applicable operating range. Therefore, the proposed approach provides a feasible alternative for the low-cost real-time orientation determination system.

Further research will focus on the calibration of MEMS devices to improve the performance of orientation determination systems. Meanwhile, some additional sensors and modules, such as the barometer and GPS, can be optionally integrated into the existing MODS. In this case, a complete orientation and position determination system would be achieved.