sensors Article

A Dual-Linear Kalman Filter for Real-Time Orientation Determination System Using Low-Cost MEMS Sensors Shengzhi Zhang 1 , Shuai Yu 1 , Chaojun Liu 1 , Xuebing Yuan 1 and Sheng Liu 1,2, * 1

2

*

School of Mechanical & Engineering, Huazhong University of Science & Technology, Wuhan 430074, China; [email protected] (S.Z.); [email protected] (S.Y.); [email protected] (C.L.); [email protected] (X.Y.) School of Power and Mechanical Engineering, Wuhan University, Wuhan 430072, China Correspondence: [email protected]; Tel.: +86-27-8754-2604; Fax: +86-27-8755-7074

Academic Editor: Gert F. Trommer Received: 14 December 2015; Accepted: 15 February 2016; Published: 20 February 2016

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. Keywords: sensor fusion; orientation determination; Kalman filter; MEMS

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–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

Sensors 2016, 16, 264; doi:10.3390/s16020264

www.mdpi.com/journal/sensors

Sensors 2016, 16, 264

2 of 17

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

Sensors 2016, 16, 264

3 of 17

Sensors 2016, 16, 264

3 of 17

In this paper, we focused on the requirements of a complete system for the low-cost orientation determination. A were novelbased dual-linear Kalman filter was designed the multi-sensor system. kinematic models on the propagation equations of the localfor gravity and geomagnetic field The kinematic models were based on the propagation equations of the local gravity and geomagnetic in the body frame. The outputs of accelerometer and magnetometer were defined as the measured field in the body The outputs of accelerometer and magnetometer the measured quantities in theframe. two independent observation models. Benefited fromwere the defined specific as design, only the quantities in the two independent observation models. Benefited from the specific design, and onlya gyro and accelerometer are enough to run the first stage filter for the attitude estimation the gyro and accelerometer are enough to run the firstheading stage filter for the attitude estimation and a magnetometer could be integrated optionally if the is needed. Considering the different 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 statistical for error sensor thetoproposed optimal orientation estimation if the sensormodels statistical is errors, assumed be white filter noise.can Theachieve proposed strategy precludes the impacts 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 of geomagnetic on by the 30.6% pitch and rollmagnetic which thedisturbances. heading is subjected to. The RMSE of attitude angles distortions are reduced under Meanwhile, owing to the attitude angles are reduced by 30.6% under disturbances. Meanwhile, owing to the reduction reduction of system complexity, total timemagnetic consumption of the proposed method is reduced by 23.8% of system complexity, total time consumption of the proposed method is reduced than than that of a standard one, which means that a higher frequency can be implementedby for23.8% the update 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 of orientation. Furthermore, the separated filter design greater flexibility andthe robustness to magnetic anomalies for the system configuration, as it is offers possible to switch on or off second filter magnetic anomalies for the system configuration, as it is possible to switch on or off the second to include or exclude the magnetometer compensation for the heading. Online experiments were filter to include the magnetometer compensation for the heading. Online experiments performed on or theexclude homemade real-time miniature orientation determination system (MODS). 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 Furthermore, we carried outto more realistic on the two-wheel self-balancing vehiclesystem drivingwhen and indoor pedestrian walking evaluate thetests performance of the designed multi-sensor indoor pedestrian walkingand to evaluate the performance of theThe designed multi-sensor systemthat when high linear accelerations angular rates were introduced. test results demonstrated the high linear accelerations and angular rates were introduced. The test results demonstrated that accuracy and stability of MODS were well guaranteed. This paper provides a feasible alternativethe for accuracy stabilitydetermination. of MODS were well guaranteed. This paper provides a feasible alternative for low-cost and orientation low-cost determination. Theorientation kinematic modeling is introduced for the multi-sensor system in Section 2. The dual-linear The kinematic modeling introduced for the multi-sensor system Sectionmodels 2. The in dual-linear filter is proposed in Section 3.isNoise characteristics are analyzed for theinsensor Section 4. filter is proposed 3. Noise characteristics are analyzed for the sensor models in Section 4. Hardware designinisSection described for the MODS in Section 5. Real-time experiments performed on the 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 homemade MODS are presented and discussed in Section 6. Finally, the forecasts are put forward for further study in Section 7. further study in Section 7.

2. System Modeling 2. System Modeling The orientation system is usually applied to determine Euler angles which are regarded as the 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 essential parameters for the navigation and motion control. So-called Euler angles are defined as rotation angles from the given inertial frame (usually called the navigation frame, denoted by n) to the rotation angles from the given inertial frame (usually called the navigation frame, denoted by n) the body frame (fixed to the moving object and denoted by b), including yaw  , pitch  , and roll to the body frame (fixed to the moving object and denoted by b), including yaw ψ, pitch θ, and roll  . The body frame xyz is attached to the orientation system. The x -axis is aligned with its forward φ. The body frame xyz is attached to the orientation system. The x-axis is aligned with its forward z -axispoints direction, the the z-axis pointstotothe thebottom bottomof ofthe thesystem system and and the the y-axis roundsup upthe theright-handed right-handed y -axisrounds direction, orthogonalcoordinate coordinateframe. frame.In Inthis thispaper, paper,the thenavigation navigationframe framennisisattached attachedto tothe thelocal locallevel levelnorth north orthogonal frame,namely namelythe theNorth, North,East, East,Down Down(NED) (NED)frame, frame,as asshown shownininFigure Figure1.1. frame,

Figure 1. Orientation of the body frame b expressed in the navigation frame n. Figure 1. Orientation of the body frame b expressed in the navigation frame n.



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

Sensors 2016, 16, 264

4 of 17

Ñ

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

Ñb

u ptq “ Cnb pφ, θ, ψq u ptq

(1)

where Cnb pφ, θ, ψq 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. Cnb can be carried out through three different separate rotations about the three y axes. Czψ , Cθ , and Cφx represent the rotation ψ angle about the z-axis, θ angle about the y-axis, and φ angle about the x-axis, respectively, which are defined as: »

cosψ — Czψ “ – sinψ 0

´sinψ cosψ 0

fi 0 ffi 0 fl , 1

»

cosθ y — Cθ “ – 0 ´sinθ

fi 0 sinθ ffi 1 0 fl , 0 cosθ

»

1 — Cφx “ – 0 0

0 cosφ sinφ

fi 0 ffi ´sinφ fl cosφ

(2)

Then: »

cosψcosθ y — Cnb “ Czψ Cθ Cφx “ – sinψcosθ ´sinθ

cosψsinθsinφ ´ sinψcosφ sinψsinθsinφ ` cosψcosφ cosθsinφ

fi cosψsinθcosφ ` sinψsinφ ffi sinψsinθcosφ ´ cosψsinφ fl cosθcosφ

(3)

The propagation equation of Cnb accords with the following equation in [23]: »

0 .n n — Cb “ Cb ¨ – r ´q

´r 0 p

fi q ffi ´p fl “ Cnb ¨ Ωbnb 0

(4)

” ıT where Ωbnb is the skew symmetric matrix of ω “ p q r , which is the angular rate of the moving object in the frame b. Differentiating Equation (1) with respect to the time: . n Ñ

u “

. n Ñb Cb ¨ u

. b n Ñ ` Cb ¨ u

(5)

Ñ

Then, the propagation equation of u in the body frame can be derived as: . b Ñ

Ñb

u “ ´Ωbnb ¨ u

(6)

The vectors of the gravity and the geomagnetic field will both yield to Equation (6): $ . b ’ Ñb & Ñ g “ ´Ωbnb ¨ g . b ’ Ñ Ñb % m “ ´Ωbnb ¨ m ” ıT ” Ñb Ñb where g “ gbx gyb gzb and m “ mbx mby the geomagnetic field in the frame b, respectively.

mbz

ıT

(7)

represent the vectors of the gravity and

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

Sensors 2016, 16, 264

5 of 17

these two approaches together for accurate orientation estimation in this paper. Unlike Zhu et al. [18], ” ı b Ñb T defining the system state vector as S “ Ñ , we define two state vectors which are the gravity m g and geomagnetic field in the body frame b in this paper, respectively: Ñb

Ñ

x1 “ g ,

Ñb

Ñ

x2 “ m

(8)

Similarly, the accelerometer and magnetometer outputs are defined as the two observation vectors corresponding to the state vectors: Ñ Ñ Ñ Ñ z1 “ a, z2 “ m (9) The unified discrete-time dynamic equation of the system state can be expressed as follows: Ñn

Ñ n ´1

x “ Φx Ñn

Ñn

Ñ n ´1

`w

Ñn

z “ x `v

Ñn

(10)

Ñn

where w and v are the process and measurement noises vectors, respectively. The system transfer matrix Φ is given as: ´ ¯ Φ “ exp ´Ωbnb ¨ ts

(11)

where ts is the sampling interval. After estimating of the state vectors, the pitch, roll, and heading are obtained by the following arc-tangent formulas: $ gyb gbx ’ ’ c ’ q q, φ “ arctanp θ “ ´arctanp ’ ´ ¯2 ` ˘ ’ gzb & 2 b b gy ` gz ’ ’ ´mby cosφ ` mbz sinφ ’ ’ ’ q % ψ “ arctanp b m x cosθ ` mby sinφsinθ ` mbz cosφsinθ

(12)

The next task is to compute the Kalman gain which is defined as: ´ ¯´1 Kn “ Pn´ H T HPn´ H T ` Rn

(13)

where Rn is the covariance matrix of measurement noise (R a for the accelerometer and Rm for the magnetometer). As the inverse of the 3 ˆ 3 order matrix HPn´ H T ` Rn is formulized, time consumption will be drastically reduced. When the orientation system is in stable states, it is easy to achieve the optimal Rn . 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–27]. The method proposed in [28] was executed for the magnetometer calibration. The strategy will be performed for Rm as follows: ˇ ˇ $ Ñ ˇ & σ2 I, if ˇˇ||Ñ ˇ m ˇ mi`1 || ´ || h ||ˇ ă ξ m Rm “ % 8, otherwise

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

Sensors 2016, 16, 264 Sensors 2016, 16, 264

6 of 17 6 of 17

   x2n  Φx2n 1  w2n 1 n n n z2  x2  vm

   x1n  Φx1n 1  w1n 1 n n n z1  x1  va

  g b and m b

 , ,  Figure Figure 2. 2. Proposed Proposed dual-linear dual-linear Kalman Kalman filter. filter.

4. 4. Noise NoiseCharacteristics Characteristics The The covariances covariances of of the the process process noise noise and and measurement measurement noise noise are are regarded regarded as as the the primary primary design design parameters to parameters to achieve achieve the the minimum minimum variance variance of of estimation estimation errors. errors. In In this this paper, paper, the the process process noise noise is is mainly derived from the angular rates measured by the gyros. We can assume that there is a small mainly derived from ” the angularırates measured by the gyros. We can assume that there is a small perturbation  ω=  p  q  r  to ω : perturbation δω “ δp δq δr to ω: ω  ω ω ω “ ω ` δω where ω  ” p q r  ıis the mean of ω . The state equation can be rewritten as: where ω “ p q r is the mean of ω. The state equation can be rewritten as:

(15) (15)

(r   r ) (q   q)   x1  fi » 0 fi fi x1  »  x     (r  0r )    ` δrq pq ` δqq x ´pr 1 ( p   p)   x2  0 ffi — ffi ffi 2  — `δrq 0p) ´pp fl x“ ´ – (pr  x3  fl – x2 fl   q q ) ( p  0 `δpq  3  ´pq ` δqq pp ` δpq 0 x fi r» q fi x1 »  0 x3  x2   pfi » 3  0 fi 0 ´r  q x1     0 x3 ´x δp   2    r ffi0—  p ffix2 —   x3 0 x1    qffi — — ffi “ ´– r 0 ´p fl – x2 fl ´– ´x3 0 x fl – δq fl  q p 0   x3   x2  x1 0  1  r    ´q p 0 x3 x2  ´x 0  δr 1 looooooooooooooooooooomooooooooooooooooooooon w

»

.

x1 — . – x2 . x »3

(16) (16)

Ñ

w



The second term on the right side of the Equation (16) is regarded as the process noise w . For a Ñ  The second term the on the right side of theofEquation (16)noise is regarded as the process noise w. For a discrete-time system, covariance matrix the process w is obtained: Ñ discrete-time system, the covariance matrix of the process noise w is obtained:

Q  L  Rg  LTT Q “ L ¨ Rg ¨ L where where

(17) (17)

»

fi 0 x3 ´x2 x  x2  ffi — 0 R g “ diag σp2 2σq2 2σr2 2, L “ – ´x3 3 0 (18) x1 fl Rg  diag  p  q  r  , L    x3 0 x1  (18) x2 ´x1 0  x2  x1 0  where σp2 , σq2 and σr2 are the variances of p, q, and r. 2 2 2 r . with the quality of stochastic models where are proposed the variances , q , and  performance The filterofis pstrongly linked r the p ,  q and of usedThe to describe the different sensors’filter noise. In this paper, sensors’ noiseofisstochastic assumed models to be a performance of the proposed is strongly linkedthe with the quality white noise according to pertinent literatures. A generalized method of wavelet moments (GMWM) used to describe the different sensors’ noise. In this paper, the sensors’ noise is assumed to be a white proposed by Guerrier et al. [12]literatures. was used toAanalyze the impacts of of different sensor error models on noise according to pertinent generalized method wavelet moments (GMWM) the filter. A was implemented to evaluate three different models: a white proposed bynumerical Guerrier etsimulation al. [12] was used to analyze the impacts of different sensor error (1) models on ”

ı

the filter. A numerical simulation was implemented to evaluate three different models: (1) a white

Sensors 2016, 16, 264

7 of 17

Sensors 2016, 16,264 264264 Sensors 2016, 16, Sensors 2016, 16,

of17 17 7 of 17 77of

noise, (2)(2) a Gauss-Markov (GM) process, (3) sum ofthree threeGM GMprocesses. processes. Firstly, we created noise, (2) a Gauss-Markov (GM) process,and and(3) (3)aaaasum sumof three GM Firstly, we created a aa a noise, (2) Gauss-Markov (GM) process, and (3) sum of three GM processes. Firstly, we created noise, aa Gauss-Markov (GM) process, and processes. Firstly, we created theoretical trajectory as shown in Figure 3. The attitude angle is expressed as follows: theoretical trajectory shown Figure3. Theattitude attitudeangle is expressed theoretical trajectory asas shown inin Figure 3.3.The The attitude angleis isexpressed expressedas asfollows: follows: theoretical trajectory as shown in Figure as follows: π  sinp0.2tq sin(0.2tt)) θ“ sin(0.2 t) 222sin(0.2 2 where t ist the time atataatasampling instant. where tthe issimulation the simulation time asampling sampling instant. where simulation time instant. where t isis the simulation time at a sampling instant.

(19) (19)(19)

(19)

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

the sensors’ sensors’ noise assumed as aa white whitethe noise. Meanwhile, the results results of of the the Gauss-Markov Gauss-Markov model is assumed as anoise white Meanwhile, results of the Gauss-Markov model are very close to the the isis noise. assumed as noise. Meanwhile, the model are very very close close to the the white whitenoise noise10model. model. are to white noise model. Model 1

5

Error Error(degree) (degree)

Error (degree)

10 0 10 Model11 -5 Model 55 -10 10 0 0 Model 2 -5 5 -5 0 -10 10 -10 10 Model22 -5 Model 55-10 10 Model 3 0 0 5 -5 -5 0 -10 10 -10 10 -5 Model33 Model 5 5-10 100 200 300 400 00 0 Time (s) -5 -5 -10 -10 100 200 based on 300 400 sensor models. Figure 4. Time-varying errors three different 00 pitch estimation 100 200 300 400 Time(s) (s) Time 2.5

Figure Time-varyingpitch pitchestimation estimationerrors errorsbased basedon on three different sensor models. Figure 4.4.4. Time-varying errors based onthree threedifferent different sensor models. Figure Time-varying pitch estimation models. Model 1 sensor Model 2 Model 3

Absolute Error (degree)

2.0

Absolute AbsoluteError Error(degree) (degree)

2.5 2.5

1.5

Model11 Model Model22 Model Model33 Model

2.0 2.0

1.0

1.5 1.5

0.5

1.0 1.0 0.0 0.5 0.5

Mean

Standard Deviation 50th Percentile

75th Percentile

0.0 Figure0.0 5. Performance comparisons of the absolute pitch estimation error. Mean Mean

StandardDeviation Deviation 50th 50thPercentile Percentile Standard

75thPercentile Percentile 75th

Figure5. 5.Performance Performancecomparisons comparisons ofthe the absolutepitch pitch estimationerror. error. Figure Figure 5. Performance comparisons of of theabsolute absolute pitchestimation estimation error.

Sensors 2016, 16, 264

8 of 17

Sensors 2016, 16, 264

8 of 17

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

(a)

(b)

Figure 6. Homemade prototype of the MODS (a) sensors layout; (b) bespoke housing. Figure 6. Homemade prototype of the MODS (a) sensors layout; (b) bespoke housing.

6. Experiments and Results 6. Experiments and Results 6.1. Noise Determination 6.1. Noise Variances Variances Determination Firstly, we weremoved removedthe themean meanofofthe theoriginal originalgyro gyrosignal signalproduced produced static tests a frequency Firstly, inin static tests at at a frequency of of 100 Hz (448,915 measurements). The time-varying errors are available and are presented the 100 Hz (448,915 measurements). The time-varying errors are available and are presented in theintime time domain in 7Figure (uppertogether panel), with together withwavelet the Haar wavelet variance and the domain in Figure (upper7panel), the Haar variance (WV) and the (WV) corresponding corresponding GMWM for this process with 95% confidence intervals (lower panel). Similar results GMWM for this process with 95% confidence intervals (lower panel). Similar results are achieved are achieved by the GMWM for a white noise process and a GM process, which validate the previous by the GMWM for a white noise process and a GM process, which validate the previous results in results 4inagain. Section again. The WV fromgive the an gyro signal for give indication for the Section The4WV computed fromcomputed the gyro signal indication thean underlying stochastic underlying stochastic processes. The GMWM estimates of the parameters and their corresponding processes. The GMWM estimates of the parameters and their corresponding 95% confidence intervals 95% confidence intervals using the WV covariances areThe summarized 1. The suitability of the using the WV covariances are summarized in Table 1. suitability in ofTable the estimated models can be estimated models can judged graphically by a matching of the empirical WV andthe theestimates parametric judged graphically by abematching of the empirical WV and the parametric WV using in WV using Table 1. the estimates in Table 1.

Sensors 2016, 16, 264 Sensors 2016, 16, 264

9 of 17 9 of 17

Figure Haar WV WV and and GMWM. GMWM. Figure 7. 7. Comparisons Comparisons (log–log (log–log scale) scale) between between the the Haar Table 1. 1. Estimated Estimated parameters parameters with with associated associated 95% 95% confidence intervals for stochastic Table confidence intervals for three three different different stochastic models with the gyro signal data. models with the gyro signal data.

Model

Model

Model 1

Model 1

Model 2

Model 2

Model 3

Model 3

Parameter

Parameter 2

 2 σ 2 2 σ  β

 12 σ12 β 1 σ21 2

 22 β22 σ3  2 β3

 32 6.2. Time Consumption Emulation 3

Estimate

IC (0.95)

4.654064e−06

(4.639158e−06; 4.668971e−06)

Estimate

4.654064e´06

IC (0.95)

(4.639158e´06; 4.668971e´06)

4.653360e−06

(4.639986e−06; 4.666734e−06)

5.342647e−02 5.342647e´02

(5.106282e−02; 5.579013e−02) (5.106282e´02; 5.579013e´02)

9.246705e−12 9.246705e´12

(5.989062e−12; 1.250435e−11) (5.989062e´12; 1.250435e´11)

4.653360e´06

(4.639986e´06; 4.666734e´06)

9.990636e´01 9.990636e−01 8.794771e´13 9.999861e´01 8.794771e−13 4.658634e´06 9.999861e−01 3.307696e´02

(9.990636e´01; 9.990636e´01)

(9.999861e−01; 9.999861e−01) (3.307696e´02; 3.307696e´02)

4.658634e−06

(4.640222e−06; 4.677046e−06)

(9.990636e−01; 9.990636e−01) (4.427838e´13; 1.316170e´12) (9.999861e´01; 9.999861e´01) (4.427838e−13; 1.316170e−12) (4.640222e´06; 4.677046e´06)

3.307696e−02 (3.307696e−02; 3.307696e−02) A time consumption test was designed to validate the effects of proposed filter for reducing the system andEmulation computing time by smaller matrix operations. We collected 31,120 row data 6.2. Timecomplexity Consumption from an existing orientation system [29] at 100 Hz which were used to run our filter in MATLAB ten Arepeatedly. time consumption test was designed to validate the effects pitch of proposed filter for reducing the times, Table 2 shows the comparisons of time-varying errors estimated by our filter system complexity computing by(Method smaller matrix We collected row data (Method A) and theand proposed filtertime in [18] B). Theoperations. proposed filter precludes31,120 the impacts of from an existing orientation system [29] at 100 Hz which were used to run our filter in MATLAB geomagnetic distortions on the pitch and roll which the heading is subjected to. As shown in Tableten 2, times, repeatedly. Table 2 shows the comparisons of time-varying pitch errors estimated by our filter the average RMS errors of proposed filter are reduced by 30.6% compared with Method B under strong (Method A) and the proposed filter mean in [18]computation (Method B).times The proposed filter the means impacts of magnetic disturbances. Meanwhile, are reduced by precludes 23.8%, which that geomagnetic distortions on the pitch and roll which the heading is subjected to. As shown in higher frequency can be implemented for the orientation updates. 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.

Sensors 2016, 16, 264

10 of 17

Sensors 2016, 16, 264

10 of 17

Table 2. Table 2. Comparisons Comparisons of of Method Method A A and and Method Method B. B. Method Method

Method A Method A Method B Method B

Mean Consumption Time ConsumptionMean Attitude Mean Attitude Error Error Mean Time 8.72 s 8.72 s 0.25° 0.25˝ 0.36˝ 11.45 s11.45 s 0.36°

6.3. MODS Evaluation Three kinds of experiments were carried out to verify the performance of the designed designed MODS. MODS. Firstly, the the static static and and dynamic dynamic tests tests for for MODS MODS were were implemented implemented on a tri-axis tri-axis turntable that allows Firstly, precise and andrepeatable repeatable tests. the MODS was mounted on a two-wheel homemade two-wheel precise tests. Then,Then, the MODS was mounted on a homemade self-balancing self-balancing vehicle for driving control. Finally, thetied MODS wasshoes tied on shoes of for pedestrian for vehicle for driving control. Finally, the MODS was on the of the pedestrian the indoor the indoor and stair-climbing experiments validate on thethe robustness on the attitude walking andwalking stair-climbing experiments to validate theto robustness attitude estimation when estimation high linear rates were introduced. high linear when accelerations and accelerations angular ratesand wereangular introduced. 6.3.1. Tri-Axis Turntable Turntable Experiments Experiments for forthe theMODS 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 majority of existing existing published published literatures, orientation systems were usually kept at the zero input point so as as to to evaluate evaluate the the static static performance. performance. In our paper, different static input ˝ and points were tested tested in in our ourstatic staticexperiments. experiments.The Theattitude attitudeangles angles were tested from ´80 to 80 were tested from −80° to˝ 80° and the ˝ ˝ ˝ the heading from to 90 turntable turned by every 10 every would be held a heading waswas from 0° to0 90°. The. The turntable waswas turned by 10° timetime and and would be held for afor few few seconds. During the dynamic test, the MODS was mounted on the running turntable which was seconds. During the dynamic test, the MODS was mounted on the running turntable which controlled to execute the pre-set pre-set motions. motions.

Figure 8. 8. Online experiments for for the the MODS. MODS. Figure Online turntable turntable experiments

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

Sensors 2016, 16, 264

11 of 17

80 80 60 60 40 40 20 20 0 0 -20 -20 -40 -40 -60 -60 -80 -80 0 0

11 of of 17 17 11

(a) (a) Roll Roll(degree) (degree)

Pitch Pitch(degree) (degree)

Sensors 2016, 2016, 16, 16, 264 264 Sensors

50 50

100 100

150 200 150 200 Time (s) Time (s)

250 250

20 20

300 300

0 0

(b) (b)

50 50

100 100

150 200 150 200 Time (s) Time (s)

250 250

300 300

(c) (c)

0 0

Yaw Yaw(degree) (degree)

80 80 60 60 40 40 20 20 0 0 -20 -20 -40 -40 -60 -60 -80 -80

-20 -20

Arc-tangent Attitude Attitude Solution Solution Arc-tangent Integrated Gyros Gyros Measurements Measurements Integrated Proposed Kalman Kalman Filter Filter Method Method Proposed Reference Reference

-40 -40 -60 -60 -80 -80 -100 -100 0 0

20 20

40 40

60 60

80 100 80 100 Time (s) Time (s)

120 140 120 140

160 180 160 180

Figure 9. Estimated Estimated orientationduring duringstatic static tests tests (a) (a) pitch angle; (b)(b) rollroll angle; (c) yaw yaw angle. Figure 9. Estimated orientation tests (a)pitch pitchangle; angle; angle; (c) yaw angle. Figure 9. orientation during static (b) roll angle; (c) angle. 60 60

(a) (a) Roll Roll(degree) (degree)

Pitch Pitch(degree) (degree)

40 40 20 20 0 0 -20 -20 -40 -40 0 0

10 10

20 20

30 30

40 50 40 50 Time (s) Time (s)

60 60

90 90

80 80

(b) (b)

10 10

20 20

30 30

40 50 40 50 Time (s) Time (s)

60 60

70 70

80 80

(c) (c)

60 60

Yaw Yaw(degree) (degree)

70 70

80 80 60 60 40 40 20 20 0 0 -20 -20 -40 -40 -60 -60 -80 -80 0 0

30 30

Arc-tangent Attitude Attitude Solution Solution Arc-tangent Integrated Gyros Gyros Measurements Measurements Integrated Proposed Kalman Kalman Filter Filter Method Method Proposed Reference Reference

0 0 -30 -30 -60 -60 -90 -90 -120 -120

0 0

10 10

20 20

30 30

40 50 40 50 Time (s) Time (s)

60 60

70 70

80 80

Figure 10. Estimated Estimated orientation duringdynamic dynamic tests tests (a) pitch angle; (b)(b) rollroll angle; (c) yaw yaw angle. Figure 10. Estimated orientation during tests(a) (a)pitch pitchangle; angle; angle; (c) yaw angle. Figure 10. orientation during dynamic (b) roll angle; (c) angle. Table 3. 3. RMS RMS errors errors of of the the MODS MODS in in the the turntable turntable tests. tests. Table

Overall, the results of MODS are quite consistent with the reference, virtually without any time Pitch that gyro-derived Roll Yaw Test State lag whatsoever in the tests. We can see clearly orientation angles deviate from the Pitch Roll Yaw Test State Static 0.045° 0.064° 0.352° Static 0.064° reference due to the inherent biases of the0.045° gyros. On the other hand, 0.352° the orientation angles estimated Dynamic 0.301° 0.386° 0.845° by the accelerometer and Dynamic magnetometer 0.301° tend to follow0.386° the reference 0.845° well in the quasi-static state, but large errors and poor reliability appear in the transient high dynamic state. Table 3 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.

Sensors 2016, 16, 264

12 of 17

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

Pitch

Roll

Yaw

Static Dynamic

0.045˝

0.064˝

0.301˝

0.386˝

0.352˝ 0.845˝

6.3.2. Experiments on the Two-Wheel Self-Balancing Vehicle Driving Sensors 2016, 16, 264

12 of 17

Sensors 2016, 16, 264 the purpose to validate

12 of 17 For 6.3.2. Experiments on the Two-Wheel the performance of the designed MODS in practical applications, we Self-Balancing Vehicle Driving mounted it6.3.2. on aExperiments homemade two-wheel self-balancing vehicle, as shown in Figure 11a. The pitch angle ontothe Two-Wheel Self-Balancing Vehicle Driving For the purpose validate the performance of the designed MODS in practical applications, we θ is one of mounted the mostit important parameters for driving the self-balancing vehicle. Thepitch servo motors will on a homemade two-wheel self-balancing vehicle, as shown 11a.applications, The angle For the purpose to validate the performance of the designed MODSininFigure practical we adjust the driving torque Td important on the wheels to keep thevehicle, dynamic balance of the vehicle body ofon the most parameters for driving the self-balancing vehicle. The servo motors  is one it mounted a homemade two-wheel self-balancing as shown in Figure 11a. The pitch angleaccording Tparameters will the driving torque as on the wheels to keep dynamic balance of The the vehicle body vehicle one thepitch most angles, important forFigure driving thethe self-balancing vehicle. servo motors to the variation of of the in 11b. At the beginning, the two-wheel  isadjust d shown TThen, will adjust driving torque the wheels to the balance vehicle body according the variation of the pitch angles, as keep shown indynamic Figure 11b.controlled At of thethe beginning, thealong the was kept static for to athefew seconds. the two-wheel vehicle was to drive d on two-wheel vehicle was kept of static for a few seconds. Then, the two-wheel vehicle was controlledthe to according to the variation the pitch angles, as shown in Figure 11b. At the beginning, preplanned route until commanded to slow down and stop. Figure 12 shows the time-varying pitch drive alongvehicle the preplanned to Then, slow down and stop. Figure shows the timetwo-wheel was keptroute staticuntil for acommanded few seconds. the two-wheel vehicle12 was controlled to and roll angles during the whole test. varying pitch and roll angles during the whole test.

drive along the preplanned route until commanded to slow down and stop. Figure 12 shows the timevarying pitch and roll angles during the whole test. MODS

MODS

Mg Mg

Td Td

Tf

mx mx

Tf

 x J r  x J r



mg mg

Ff Ff

Mg Mg

x x

N

(b)N

(a)

(a) (b) fixed on the vehicle; Figure 11. Tests on the two-wheel self-balancing vehicle (a) with MODS

Figure 11. (b) Tests on the two-wheel self-balancing vehicle (a) with MODS fixed on the vehicle; (b) schematic diagram for two-wheel the vehicle driving. Figure 11. Tests on the self-balancing vehicle (a) with MODS fixed on the vehicle; schematic diagram fordiagram the vehicle (b) schematic for the driving. vehicle driving. 3.0

30

Proposed Kalman Filter Method Reference Proposed Kalman Filter Method Reference

25 30

Roll (degree) Roll (degree)

(degree) PitchPitch (degree)

20 25 15 20 10 15 5 10 05 -50 -10 -5 -10

2.5 2.0 2.0 1.5 1.5 1.0 1.0 0.5 0.5 0.0

0

5

0

5

10

15 Time (s) 10 15 (a)Time (s)

Proposed Kalman Filter Method Reference Proposed Kalman Filter Method Reference

3.0 2.5

20

25

30

20

25

30

0.0

0

5

10

0

5

10

15 Time (s) 15 (b) Time (s)

20

25

30

20

25

30

(a) attitude angle during two-wheel self-balancing vehicle (b) test (a) pitch angle; Figure 12. Estimated (b) roll angle. Figure 12. Estimated attitude angle during two-wheel self-balancing vehicle test (a) pitch angle; 12. (b) Estimated roll angle. attitude angle during two-wheel self-balancing vehicle test (a) pitch angle;

Figure roll angle. The pitch angle changes rapidly at the time of 11 s and 23 s corresponding to the starting and

(b)

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

The and stopping of the two-wheel vehicle, respectively. The variations of pitch angles are relatively stable whole test. Indoor Pedestrian Walking and Stair-Climbing Experiments during the6.3.3. vehicle running. Meanwhile, the roll angle changes stably within 1˝ during the whole test. 6.3.3.For Indoor Pedestrian Walking Stair-Climbing Experiments the further validation of and the accuracy and stability of the MODS in more realistic motions, indoor pedestrian walking and stair-climbing tests were carried outMODS in our college shown For the further validation of Stair-Climbing the accuracy and stability of the in morebuilding, realisticas motions, 6.3.3. Indoor Pedestrian Walking and Experiments in Figure 13. The walking MODS was tied on the shoes of thecarried pedestrian. During thebuilding, waking as tests, the indoor pedestrian and stair-climbing tests were out in our college shown For the validation the accuracy and stability of theon MODS inThe more pedestrian was required walk straight along the of preplanned route the floor. floorrealistic tiles can in further Figure 13. The MODStoof was tied on the shoes the pedestrian. During the waking tests, the

motions, pedestrianwalking was required walk straight along thewere preplanned route floor. The floor tiles can as shown indoor pedestrian andtostair-climbing tests carried outoninthe our college building,

Sensors 2016, 16, 264

13 of 17

Sensors 2016, 16, 264

13 of 17

Sensors 2016, 16, 264

13 of 17 in Figure be 13.regarded The MODS was tied on the shoes of the pedestrian. During the waking tests, the as the natural tags to repeat the walking tests. Raw sensor data and orientation angles pedestrianwere was required to walk straight along the preplanned route on the floor. The floor tiles can be collected the MODS. shownthe in Figure 14,tests. norms of the walking areangles much be regarded asfrom the natural tags As to repeat walking Raw sensor data accelerations and orientation Sensors 2016, 16, 264 13 of 17 higher than thefrom gravity, even close to walking thein maximum range ofwalking accelerometer (5 g). Similarly, were thetoMODS. Asthe shown Figure 14,scale norms of the are much regarded as thecollected natural tags repeat tests. Raw sensor data accelerations and orientation angles were norms of walking angular ratesclose are close the maximum range of gyros (450°/s). higherthe than gravity, even to thetomaximum scale range of accelerometer g). Therefore, Similarly, collected from MODS. As tags shown in Figure 14, norms ofscale the walking accelerations are much be regarded as thethe natural to repeat the walking tests. Raw sensor data and (5orientation angleshigher high accelerations angular rates are maximum introducedscale intorange the proposed filter to Therefore, verify the normslinear of walking angularand rates are close to the of gyros (450°/s). collectedeven from the MODS. Asmaximum shown in Figure 14, norms of the walking accelerations are much thanwere the gravity, close to the scale range of accelerometer (5 g). Similarly, norms of robustness the attitudeand estimation. cycles are shown clearly byfilter identifiable high linear of accelerations angular The rateswalking are introduced into the proposed to verifypeak the ˝ higher than the gravity, even close to the maximum scale range of accelerometer (5 g). Similarly, walking angular rates are close to maximum scale range ofshown gyrosclearly (450 by /s). Therefore, high linear points which be applied tothe indoor pedestrian navigation and human health monitoring. robustness ofcan the attitude estimation. The walking cycles are identifiable peak norms points of walking angular rates close to the maximum scale range oftogyros (450°/s). Therefore, Figure 15can shows the real-time attitude angles of theproposed MODS walking tests. Similar to the which be rates applied toare indoor pedestrian navigation andduring human health monitoring. accelerations and angular are introduced into the filter verify the robustness of the high linear accelerations and angular rates are introduced into the proposed filter to verify the variations of accelerations and angular rates in Figure 14, attitude angles change periodically with Figure 15 shows the real-time attitude angles of the MODS during walking tests. Similar to the attitude estimation. The walking cycles are shown clearly by identifiable peak points which can be the pedestrian waking estimation. cycles. MODS can provide attitude forwith thepeak variations of accelerations and The angular in Figure 14,stable attitude angles changeestimation periodically robustness of the attitude Therates walking cycles are enough shown clearly by identifiable applied topedestrian indoor pedestrian navigation and human health monitoring. walking. the pedestrian waking cycles. The MODS can provide stable enough attitude estimation for the

points which can be applied to indoor pedestrian navigation and human health monitoring. pedestrian walking. 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.

(a) (a)

(b) (b)

Figure 13. (a) Indoor pedestrian walking tests and (b) stair-climbing tests.

Figure 13. (a)13. Indoor pedestrian tests and stair-climbing Figure (a) Indoor pedestrianwalking walking tests and (b) (b) stair-climbing tests. tests. Norms of Walking Acceleration

40

2

2 Acceleration Acceleration (m/s(m/s ) )

40

(a)

30 30

Angular Rate (rad/s) Angular Angular (rad/s) RateRate (rad/s)

Norms of Walking Acceleration

40

2

Acceleration (m/s )

20

Norms of Walking Angular Rate

8

Norms of Walking Angular Rate

6

(b)

6

Figure 13. (a) Indoor pedestrian walking tests and (b) stair-climbing tests. 4

20 20

Norms of Walking Acceleration

10 10

30

8

25

26

27

25

26

27

Time (s)

(a)Time (s) (a)

28

29

30

28

29

30

4 2

Norms of Walking Angular Rate

82 0

60

25

26

25

26

4

27

28 Time (s) 27 28 (b)Time (s)

29

30

29

30

(b)

Figure 14. Inertial sensors measurements during walking (a) acceleration; (b) angular rate.

2 Figure 14. Inertial sensors measurements during walking (a) acceleration; (b) angular rate.

Figure 14. Inertial sensors measurements during walkingProposed (a) acceleration; (b) angular rate. Kalman Filter Method 100 (degree) PitchPitch (degree)

10

Reference Proposed Kalman Filter Method Reference

0

100 80

80 Figure the attitude angles of the MODS during walking tests. Similar to 60 25 15 shows 26 27 real-time 28 29 30 25 26 27 28 29 30 Time (s) and angular rates in Figure 14, attitude Time (s) 60 the variations of accelerations angles change periodically 40 40 (a) 20 (b) attitude estimation for the with the pedestrian waking cycles. The MODS can provide stable enough 20 0 pedestrian walking. Figure 14. Inertial sensors measurements during walking (a) acceleration; (b) angular rate. 0

5

6

7

8

5

6

7

8

Pitch (degree)

100

9 Time (s) 9 (a) Time (s)

10

10

(a)

11

12

13

11 12 13 Proposed Kalman Filter Method Reference

Figure 15. Cont.

80

Figure 15. Cont.

60 40 20 0 5

6

7

8

9 Time (s)

(a) Figure 15. Cont.

Figure 15. Cont.

10

11

12

13

Sensors 2016, 16, 264

14 of 17

Sensors 2016, 16, 264

14 of 17

Proposed Kalman Filter Method Reference

200 150 Roll (degree)

Sensors 2016, 16, 264 100

14 of 17

50 0 Proposed Kalman Filter Method Reference

-50 200 -100 150 Roll (degree)

-150 100 -200

50

5

6

0

7

8

9

10

11

12

13

Time (s)

-50

(b)

-100 -150

-200 Figure 15. Estimated attitude angle during walking (a) pitch angle; (b) roll angle.

5 6 7angle 8during9 walking 10 12 13 Figure 15. Estimated attitude (a) 11pitch angle; (b) roll angle. Time (s)

(b) was required to climb three floors including six During the stair-climbing tests, the pedestrian half floors and15.six corners walking are corresponding to the red segments and the six During theclimbing stair-climbing tests,attitude the pedestrian was required to climb floors including Figure Estimated anglewhich during walking (a) pitch angle; (b) roll three angle. blue segments the six time-varying curves of the accelerations and angular rates Figure 16.and We can half floors climbingofand corners walking which are corresponding to the redinsegments the blue During stair-climbing tests, required to are climb three larger floors including find out that thethe variation ranges of the thepedestrian climbing was accelerations than 16. thatsix of can the find segments of the time-varying curves of the accelerations and angularmuch rates in Figure We half accelerations, floors climbing but and six corners walking which are corresponding to the red an segments andtendency. the walking the variation ranges of the angular rates show inverse out that the variation ranges of the climbing ofcan the walking blue segments of the time-varying curves of accelerations the accelerations are and much angular larger rates in than Figurethat 16. We Therefore, the proposed filter will rely more on the gyro outputs during the pedestrian walking, find out variation ranges thethe climbing accelerations are much larger than that of theTherefore, accelerations, butthat thethe variation rangesofof angular rates show an inverse tendency. whilewalking the accelerometer outputs are considered more in the quasi-static attitude updates. accelerations, but the variation ranges of the angular rates show an inverse tendency.

the proposed filter will rely more on the gyro outputs during the pedestrian walking, while the Therefore, the proposed filter will rely more on the gyro outputs during the pedestrian walking, accelerometer 60 outputs are considered more in the quasi-static attitude updates. Climbing

50 40 2

40

20

30

10

20

0

0

Climbing Stairs Walking

Climbing

50

30

-10

Walking

60

Norm of Acceleration (m/s )

2

Norm of Acceleration (m/s )

while the accelerometer outputs are considered more in the quasi-static attitude updates. Climbing Stairs

10

Walking

0 -10

10 0

Walking

20 10

20

30 30

40

Time (s) 40

50 50

60 60

70 70

Time (s)

(a) (a)

6

6

4

4

2

2

0

0

Climbing Climbing

0 Walking

-2

-2

Climbing Stairs

Climbing Stairs Walking Walking

8

Norm of Angular Rate (rad/s)

Norm of Angular Rate (rad/s)

8

0

Walking

10

10

20

20

30

30

40

Time (s)

40

50

60

50

70

60

70

Time (s)

(b)

(b) Figure 16. Inertial sensors measurements during stair-climbing (a) acceleration; (b) angular rate. Figure 16. Inertial sensors measurements during stair-climbing (a) acceleration; (b) angular rate.

Figure 16. Inertial sensors measurements during stair-climbing (a) acceleration; (b) angular rate. 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 Figure 17 shows real-timeattitude attitude angles angles estimated during the the pedestrian Figure 17 shows thethe real-time estimatedbybythe theMODS MODS during pedestrian of climbing stairs. The results are very consistent with the realistic motions because the pedestrian’s

stair-climbing pitch and anglesofofthe thewalking walking have have aa larger than that stair-climbing tests.tests. TheThe pitch and rollroll angles largervariation variationrange range than that of of climbing stairs. The results are very consistent with the realistic motions because the pedestrian’s 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.

Sensors 2016, 16, 264 Sensors 2016, 16, 264

15 of 17 15 of 17

feet cancan stretch completely when walking while they will be be blocked byby thethe steps when climbing feet stretch completely when walking while they will blocked steps when climbing stairs. Figure 18 is the close-ups relevant to the pitch and roll angles in Figure 17, which shows Sensors 2016, 16, 264 15 of an 17 an stairs. Figure 18 is the close-ups relevant to the pitch and roll angles in Figure 17, which shows excellent convergence and dynamic performance. excellent convergence and dynamic performance. 100 100

60 40 20 0

Proposed Kalman Filter Method Proposed Kalman Filter Method Reference Reference

200 200

80 100 100

Roll (degree) Roll (degree)

Pitch (degree) Pitch (degree)

80

Proposed Kalman Filter Method Proposed Kalman Filter Method Reference Reference

60

0

40

-100 -100

20 00

0

0

10

20

10

30 40 30 (s) 40 Time Time (s)

20

50

50

60

60

70

70

-200 -200 0

0

10

10

(a)(a)

20

20

30 40 30 (s) 40 Time Time (s)

50

50

60

60

70

70

(b)(b)

Figure 17. Estimated attitude angle during stair-climbing (a) pitch angle; (b) roll angle. Figure 17.17. Estimated attitude angle during stair-climbing (a)(a) pitch angle; (b)(b) rollroll angle. Figure Estimated attitude angle during stair-climbing pitch angle; angle. 100

Pitch (degree)

Pitch (degree)

80 60 40 20

80 60 40 20

0 20 0 20

100 80

Roll (degree)

60

Roll (degree)

Proposed Kalman Filter Method Proposed Kalman Filter Method Reference Reference

100

40 20 0

-20

25

25

30 30 Time (s) Time (s)

35

35

40

40

(a)(a)

100

Proposed Kalman Filter Method Proposed Kalman Filter Method Reference Reference

80 60 40 20 0 -20

-40 20-40 20

25

25

30 30 Time (s) Time (s)

35

35

40

40

(b)(b) Figure 18.18. Close-ups relevant to to thethe estimated attitude angle during stair-climbing (a)(a) pitch angle; Figure Close-ups relevant estimated attitude angle during stair-climbing pitch angle; Figure 18. Close-ups relevant to the estimated attitude angle during stair-climbing (a) pitch angle; (b) (b)(b) rollroll angle. angle. roll angle.

4. Conclusions 4. Conclusions 7. Conclusions In In this paper, a novel dual-linear Kalman filter was designed for thethe orientation determination this paper, a novel dual-linear Kalman filter was designed orientation determination In this paper, a novel dual-linear Kalman filter was designed forfor the orientation determination system using low-cost MEMS-based sensors. The propagation equations of the local gravity and system using low-cost MEMS-based sensors. The propagation equations of the local gravity and 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 geomagnetic field in frame b are used to establish the dynamic models. The accelerometer and geomagnetic field in frame b are used to two establish the dynamic models. The accelerometer and magnetometer outputs areare defined as as the measured quantities in in thethe same, butbut independent, magnetometer outputs defined the two measured quantities same, independent, magnetometer outputs are defined as the two measured quantities in the same, but independent, observation models. The proposed strategies precludes thethe impacts of of magnetic disturbances onon thethe observation models. The proposed strategies precludes impacts magnetic disturbances observation models. The proposed strategies precludes the impacts of magnetic disturbances on the pitch and rollroll angles which thethe heading is subjected to.to. The RMSE of of estimated attitude angles are pitch and angles which heading is subjected The RMSE estimated attitude angles are pitch and roll angles which the heading is subjected to. The RMSE of estimated attitude angles are reduced byby 30.6% compared with a standard filter under magnetic disturbances. The proposed filter reduced 30.6% compared with a standard filter under magnetic disturbances. The proposed filter reduced by 30.6% compared with a standard filter understatistical magneticerror disturbances. The proposed filter can achieve optimal orientation estimation if the sensor is assumed to to be white noise. can achieve optimal orientation estimation if the sensor statistical error is assumed be white noise. can achieve optimal orientation estimation if the sensor statisticaliserror is assumed tocompared be white noise. Owing to the reduction of system complexity, time consumption reduced by 23.8% with Owing to the reduction of system complexity, time consumption is reduced by 23.8% compared with to the reduction of system complexity, time consumption is reducedfor bythe 23.8% compared with a aOwing standard filter, which means that higher frequency cancan be be implemented orientation updates. a standard filter, which means that higher frequency implemented for the orientation updates. 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

Sensors 2016, 16, 264

16 of 17

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. Acknowledgments: The supports of the National High Technology Research and Development Program (863 Program) under Grant 2013AA041105 of Ministry of Science and Technology of PR China are highly appreciated. The authors also wish to thank all the friends and colleagues who have made a great contribution to this research at Huazhong University of Science & Technology and Wuhan University. Author Contributions: Shengzhi Zhang proposed the original sensor fusion algorithm and wrote this paper; Shuai Yu developed the homemade prototype of MODS; Chaojun Liu gave some valuable suggestions; Xuebing Yuan designed the experiments; Sheng Liu revised the paper. Conflicts of Interest: The authors declare no conflict of interest.

References 1. 2. 3.

4. 5.

6. 7. 8. 9. 10. 11. 12. 13.

De Marina, H.G.; Pereda, F.J.; Giron-Sierra, J.M.; Espinosa, F. UAV attitude estimation using unscented kalman filter and TRIAD. IEEE Trans. Ind. Electron. 2012, 59, 4465–4474. [CrossRef] Wei, S.; Xu, A.G.; Yang, G. Strapdown gyrocompass algorithm for AUV attitude determination using a digital filter. Measurement 2013, 46, 815–822. Berger, C.; Al Mamun, M.A.; Hansson, J. COTS-architecture with a real-time OS for a self-driving miniature vehicle. In Proceedings of the Workshop on Architecting Safety in Collaborative Mobile Systems (ASCoMS) of the 32nd International Conference on Computer Safety, Reliability and Security, Toulouse, France, 24–27 September 2013; pp. 1–12. Peng, H.; Cardou, P.; Desbiens, A.; Gagnon, E. Estimating the orientation of a rigid body moving in space using inertial sensors. Mutibody Syst. Dyn. 2014, 2014, 1–27. Al Delail, B.; Weruaga, L.; Zemerly, M.J.; Ng, J.W.P. Indoor localization and navigation using smartphones augmented reality and inertial tracking. In Proceedings of the IEEE International Conference on Electronics, Circuits, and Systems, Abu Dhabi, UAE, 8–11 December 2013; pp. 929–932. Foxlin, E. Pedestrian tracking with shoe-mounted inertial sensors. IEEE Comput. Graph. Appl. 2005, 25, 38–46. [CrossRef] [PubMed] Öhberg, F.; Lundström, R.; Grip, H. Comparative analysis of different adaptive filters for tracking lower segments of a human body using inertial motion sensors. Meas. Sci. Technol. 2013, 24, 50–61. [CrossRef] Yuan, Q.; Chen, I.M. Localization and velocity tracking of human via 3 IMU sensors. Sens. Actuators A Phys. 2014, 212, 25–33. [CrossRef] Farrell, J.; Barth, M. The Global Positioning System and Inertial Navigation; McGraw-Hill: New York, NY, USA, 1999. Kusuda, Y.; Takahashi, M. Feedback control with nominal inputs for agile satellites using control moment gyros. J. Guid. Control Dynam. 2012, 34, 1209–1218. [CrossRef] Bekkeng, J.K.; Psiaki, M.L. Attitude estimation for sounding rockets using microelectromechanical system gyros. J. Guid. Control Dynam. 2012, 31, 533–542. [CrossRef] Guerrier, S.; Skaloud, J.; Stebler, Y.; Victoria-Feser, M.P. Wavelet-variance-based estimation for composite stochastic processes. J. Am. Stat. Assoc. 2013, 108, 1021–1030. [CrossRef] [PubMed] Yun, X.; Bachmann, E.; Mcghee, R. A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements. IEEE Trans. Instrum. Meas. 2008, 57, 638–650.

Sensors 2016, 16, 264

14. 15.

16. 17. 18. 19. 20. 21. 22. 23. 24. 25. 26. 27.

28. 29.

17 of 17

Searcy, J.D.; Pernicka, H.J. Magnetometer-only attitude determination using novel two-step Kalman filter approach. J. Guid. Control Dynam. 2012, 35, 1693–1701. [CrossRef] Renaudin, V.; Combettes, C. Magnetic, acceleration fields and gyro quaternion (MAGYQ)-based attitude estimation with smartphone sensors for indoor pedestrian navigation. Sensors 2014, 14, 22864–22890. [CrossRef] [PubMed] Yoo, T.S.; Hong, S.K.; Yoon, H.M.; Park, S. Gain-scheduled complementary filter design for a MEMS based attitude and heading reference system. Sensors 2011, 11, 3816–3830. [CrossRef] [PubMed] Ma, D.M.; Shiau, J.K.; Wang, I.C.; Lin, Y.H. Attitude determination using a MEMS-based flight information measurement unit. Sensors 2012, 12, 1–23. [CrossRef] [PubMed] Zhu, R.; Sun, D.; Zhou, Z.; Wang, D. A linear fusion algorithm for attitude determination using low cost MEMS-based sensors. Measurement 2007, 40, 322–328. [CrossRef] Han, S.; Wang, J. A novel method to integrate IMU and magnetometers in attitude and heading reference systems. J. Navig. 2011, 64, 727–738. [CrossRef] Li, W.; Wang, J. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems. J. Navig. 2013, 66, 99–113. [CrossRef] Sabatelli, S.; Galgani, M.; Fanucci, L.; Rocchi, A. A double-stage Kalman filter for orientation tracking with an integrated processor in 9-D IMU. IEEE Trans. Instrum. Meas. 2013, 62, 590–598. [CrossRef] Yuan, X.; Yu, S.; Zhang, S.; Wang, G.; Liu, S. Quaternion-based unscented Kalman filter for accurate indoor heading estimation using wearable multi-sensor system. Sensors 2015, 15, 10872–10890. [CrossRef] [PubMed] Titterton, D.H.; Weston, J.L. Strapdown Inertial Navigation Technology, 2nd ed.; Institution of Electronical Engineers: Reston, VA, USA, 2004. Sabatini, A.M. Kalman-filter-based orientation determination using inertial/magnetic sensors: observability analysis and performance evaluation. Sensors 2011, 11, 9182–9206. [CrossRef] [PubMed] Renaudin, V.; Afzal, M.H.; Lachapelle, G. Complete triaxis magnetometer calibration in the magnetic domain. J. Sens. 2010, 115, 23–59. [CrossRef] Li, X.; Li, Z. A new calibration method for tri-axial field sensors in strap-down navigation systems. Meas. Sci. Technol. 2012, 23, 2852–2855. [CrossRef] Metge, J.; Mégret, R.; Giremus, A.; Berthoumieu, Y.; Décamps, T. Calibration of an inertial-magnetic measurement unit without external equipment, in the presence of dynamic magnetic disturbances. Meas. Sci. Technol. 2014, 25, 125106–125122. [CrossRef] Zhang, Z.; Yang, G. Calibration of miniature inertial and magnetic sensor units for robust attitude estimation. IEEE Trans. Instrum. Meas. 2014, 63, 711–718. [CrossRef] Xsens Technologies B.V. MTi300 datasheet. Available online: https://www.xsens.com/products/mti-100-series/ (accessed on 17 February 2016). © 2016 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons by Attribution (CC-BY) license (http://creativecommons.org/licenses/by/4.0/).