sensors Article
A Novel 3D Pedestrian Navigation Method for a Multiple Sensors-Based Foot-Mounted Inertial System Wei Yang, Chundi Xiu *, Jianmin Zhang and Dongkai Yang School of Electronic and Information Engineering, Beihang University, Beijing 100083, China;
[email protected] (W.Y.);
[email protected] (J.Z.);
[email protected] (D.Y.) * Correspondence:
[email protected]; Tel.: +86-10-8231-7222 Received: 19 September 2017; Accepted: 17 November 2017; Published: 22 November 2017
Abstract: In this paper, we present a novel method for 3D pedestrian navigation of foot-mounted inertial systems by integrating a MEMS-IMU, barometer, and permanent magnet. Zero-velocity update (ZUPT) is a well-known algorithm to eliminate the accumulated error of foot-mounted inertial systems. However, the ZUPT stance phase detector using acceleration and angular rate is threshold-based, which may cause incorrect stance phase estimation in the running gait pattern. A permanent magnet-based ZUPT detector is introduced to solve this problem. Peaks extracted from the magnetic field strength waveform are mid-stances of stance phases. A model of peak-peak information and stance phase duration is developed to have a quantitative calculation method of stance phase duration in different movement patterns. Height estimation using barometer is susceptible to the environment. A height difference information aided barometer (HDIB) algorithm integrating MEMS-IMU and barometer is raised to have a better height estimation. The first stage of HDIB is to distinguish level ground/upstairs/downstairs and the second stage is to calculate height using reference atmospheric pressure obtained from the first stage. At last, a ZUPT-based adaptive average window length algorithm (ZUPT-AAWL) is proposed to calculate the true total travelled distance to have a more accurate percentage error (TTDE). This proposed method is verified via multiple experiments. Numerical results show that TTDE ranges from 0.32% to 1.04% in both walking and running gait patterns, and the height estimation error is from 0 m to 2.35 m. Keywords: indoor positioning; MEMS-IMU; barometer; permanent magnet; zero-velocity update; height estimation; magnetic field strength; extended Kalman filter
1. Introduction With the rapid development of location-based services (LBSs), multiple pedestrian navigation systems are being widely studied, which can roughly be divided into indoor and outdoor scenarios. Global navigation satellites systems (GNSSs), including GPS, GLONASS, Beidou and GALILEO, are a good method for outdoor navigation [1]. They provide global services and users can get accurate position, velocity and time (PVT) in the open air. However, GNSS signal availability is limited in indoors and urban canyons, so some alternative techniques need to be studied [2]. Several infrastructure-based alternative methods such as WIFI [3–5], Bluetooth [6,7], RFID [8], Ultra-Wide Band (UWB) [9,10], LED [11] and ultrasound [12] have been studied over the past years. These methods can provide sufficient positioning accuracy for LBS in normal situations. However, in some special scenarios such as fire emergency relief, infrastructure-based indoor positioning systems are not available. The inertial navigation system (INS) is an infrastructure-free one that uses inertial sensors such as accelerometers and gyroscopes which do not require external reference and installation beforehand [13]. Furthermore, the development of micro-electromechanical system Sensors 2017, 17, 2695; doi:10.3390/s17112695
www.mdpi.com/journal/sensors
Sensors 2017, 17, 2695
2 of 30
inertial measurement unit (MEMS-IMU) has facilitated the production of inexpensive, lightweight and small-size inertial sensors with low power consumption [14]. MEMS-IMU can provide high accuracy positioning for a short time. However, the drift accumulates very fast, which over time leads to large positioning errors. In order to solve this problem, some researchers have utilized the zero-velocity update (ZUPT) algorithm, which is based on the status of the left and right foot. In other words, the left and right foot alternate between a stationary stance phase and a moving swing phase. The system detects the stance phase and applies ZUPT as pseudo measurements into the extended Kalman filter (EKF) navigation error corrector. This allows the EKF to correct the velocity error during each gait cycle, breaking the cubic-in-time error growth and replacing it with an error accumulation that is linear in the number of steps [15]. Many researchers have improved the ZUPT-aided EKF method. In [16], zero angular rate update (ZARU), heuristic heading reduction (HDR) and electronic compass are used for mitigating heading drift. In [17,18], a module consisting of both hardware and embedded software is provided. In [19], details of how to implement a foot-mounted MEMS-IMU positioning system are described. The elegance of ZUPT lies in the fact that the foot swings to stance phase periodically during most types of human locomotion, such as walking, running, ascending and descending stairs. The stance phase is the time period during which the ZUPT can be applied to the system with the foot in stationary status [20]. Different ZUPT detectors can be classified into two categories depending on the equipment used. The first kind is using MEMS-IMU alone and the other is integrated with additional devices. MEMS-IMU-based ZUPT detectors utilize acceleration and angular rate which represent the state of motion of the pedestrian directly. Most ZUPT detectors utilize acceleration and angular rate obtained from MEMS-IMU. In [21,22], the stance phase estimation problem is formalized as a binary hypothesis testing problem. Four zero-velocity detectors including the acceleration moving variance detector, the acceleration magnitude detector, the angular rate energy detector, and the stance hypothesis optimal detector are evaluated. In [14,16], three conditions including the magnitude of the acceleration, the local acceleration variance and the magnitude of the gyroscope are used for foot stationary detection. The three conditions must be satisfied simultaneously. The structure of this kind of ZUPT detectors is simple and they have a good stance phase detection accuracy in walking, but when the pedestrian runs, the performance is limited because that this kind of method is threshold-based and the running threshold is not the same compared with walking. The second kind of ZUPT detector uses additional devices such as a shoe-embedded radar [23], a shoe-embedded RF sensor [24], or a shoe-embedded high resolution pressure sensor [25]. Generally speaking, this can bring better performance, but the complexity of system also increases. A permanent magnet-based stance phase detection method was developed in [26]. The authors attached a permanent magnet on one foot and an MEMS-IMU on the other foot. The walking pattern is measured based on the magnetic field strength of the permanent magnet, which changes very rapidly when the foot is in the mid-stance phase. The advantage of a permanent magnetic-based ZUPT detector is that it is not threshold-based and can be applied in both walking and running gait patterns. Compared with other ZUPT detectors using additional devices, mag-ZUPT has a more compact structure in that the measurement device is still a MEMS-IMU and the permanent magnet does not need a power supply. However, stance phase duration is not considered in [26]. Mid-stance should be expanded to stance phase with stance phase duration knowledge. In [27–29], the percentage of time involved in the stance is approximately 24.8%. However, we have performed some experiments and found that the stance phase duration is in fact associated with different movement patterns and the 24.8% value is only suitable for walking gait patterns. Stance phase duration is closely related with movement patterns. In order to develop a model of stance phase duration and movement pattern, peak-peak period information obtained from mag-ZUPT is used. Every mid-stance is called one peak and peak-peak period information depicts the frequency of stance phase which is relative with the stance phase duration. Compared with [26], we have a further research of the model of mag-ZUPT. Relationship of peak-peak information and stance phase duration is developed to have a quantitative calculation method of different movement patterns.
Sensors Sensors 2017, 2017, 17, 17, 2695 2695
33 of of 30 30
The height estimation suffers more from the MEMS-IMU drifts. MEMS-barometers are cheap and useful sensors to be implemented into thethe MEMS-IMU positioning system [30]. However, the The height estimation suffers more from MEMS-IMU drifts. MEMS-barometers are cheap barometer heavily influenced by In order to solve problem, a and usefuloutput sensorsisto be implemented intothe theenvironment MEMS-IMU[31,32]. positioning system [30].this However, the height difference barometer algorithm is developed. HDIB based on the barometer outputinformation-aided is heavily influenced by the(HDIB) environment [31,32]. In order to solveis this problem, principle that the short time accuracy of INS output is very high andiscan be used to distinguish a height difference information-aided barometer (HDIB) algorithm developed. HDIB is basedthe on patterns of a that person’s vertical walking theand level ground, and the principle the short timemovement accuracy ofincluding INS output is veryon high can be usedascending to distinguish descending This prior information canincluding be used walking to filter on thethe height results from the patternsstairs. of a person’s vertical movement levelestimation ground, ascending and adescending barometer. stairs. This prior information can be used to filter the height estimation results from Percentage error of total travelled distance (TTDE) is used to evaluate the positioning accuracy a barometer. of INS [33]. It iserror calculated from positioning error and totaltotravelled distance in closed paths. Percentage of total travelled distance (TTDE) is used evaluate the positioning accuracy of However, that from the sampling frequency MEMS-IMU is very high (usually than INS [33]. Itbecause is calculated positioning error andoftotal travelled distance in closed paths.more However, 50 Hz), the trajectory calculated from the navigation system is the more trace of the 50 foot which because that the sampling frequency of pedestrian MEMS-IMU is very high (usually than Hz), the is mountedcalculated with MEMS-IMU. The lengthnavigation of this trajectory than trajectory which trajectory from the pedestrian systemisislonger the trace ofthe theactual foot which is mounted leads to the result The that length TTDE of is this smaller than the reality.than In this paper,trajectory we develop a ZUPT with MEMS-IMU. trajectory is longer the actual which leads based to the adaptive method is a more accurate and universal total result thataverage TTDE iswindow smaller length than the reality. (ZUPT-AAWL). In this paper, weItdevelop a ZUPT based adaptive average travelled distance calculation method. It is a more accurate and universal total travelled distance window length method (ZUPT-AAWL). In conclusion we explore three questions in this paper: calculation method. In conclusion we explore three questions in this paper: • How to use the permanent magnet in running movement to get a more accurate ZUPT How to use the permanent magnet in running movement to get a more accurate ZUPT estimation? • estimation? •• How How to to get a more accurate height estimation fusing MEMS-IMU and barometer? •• How to How to get a more accurate TTDE? In In order order to solve these problems, we developed developed a novel 3D pedestrian navigation navigation method method integrating integrating MEMS-IMU, MEMS-IMU, barometer and permanent magnet which is shown in Figure 1.
ZUPT-AAWL Permanent Magnet MEMS-IMU
System Eveluation
posx, posy accx, accy, accz gyrx, gyry, gyrz
magx, magy, magz mag-ZUPT
TTDE
Error estimation
INS
cali_posz posz
Error state vector
HDIB
Barometer
EKF
Figure Figure1.1.Structure Structureof ofthis thisnovel novel3D 3Dpositioning positioningsystem. system.
This systemcontains contains three sensors including one MEMS-IMU, one barometer and one This system three sensors including one MEMS-IMU, one barometer and one permanent permanent magnet. In fact, magnetometer is not part of IMU. Considering that usually accelerometer, magnet. In fact, magnetometer is not part of IMU. Considering that usually accelerometer, gyroscope gyroscope and magnetometer are integrated in one device, magnetometer is regarded as part of and magnetometer are integrated in one device, magnetometer is regarded as part of MEMS-IMU MEMS-IMU in this paper for convenience. MEMS-IMU collects the 3-axis accelerometer, gyroscope in this paper for convenience. MEMS-IMU the 3-axis accelerometer, gyroscope and are , collects and magnetometer readings which , b b, b , , and b , , , b b b b b magnetometer readings which are a x , ay , az , ωx , ωy , ωz and magx , magy , magz , respectively. respectively. The magnetometer readings are the composite magnetic field strength of the earth The magnetometer readings are the composite magnetic field strength of the earth magnetic field magnetic field and the permanent magnet which is used to detect the mid-stance phase and then and the permanent magnet which is used to detect the mid-stance phase and then expanded to the expanded to the stance phase. Unlike acceleration and angular rate, air pressure value obtained from stance phase. Unlike acceleration and angular rate, air pressure value obtained from barometer is only barometer is only one dimension. one dimension.
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
4 of 30 4 of 30
The foot-mounted MEMS-IMU The basic basic structure structure of of a a foot-mounted MEMS-IMU positioning positioning system system is is INS-EKF-ZUPT INS-EKF-ZUPT (IEZ). (IEZ). Stance phase information obtained from mag-ZUPT is used to get the error and Stance phase information obtained from mag-ZUPT is used to get the error estimation estimation and transmitted transmitted to EKF error to calculate error state INSthe calculates the positioning (pos x,posy,posz). to EKF to calculate state vector. INS vector. calculates positioning results (posxresults ,posy ,pos z ). (posx ,posy ) (pos x,posy) is the planar positioning result and posz is the vertical positioning result. In order to get a is the planar positioning result and posz is the vertical positioning result. In order to get a better height better height estimation,isaused barometer used in this system. The difference information of posz can estimation, a barometer in this is system. The difference information of pos z can distinguish the distinguish the person’s patterns vertical of the person’s vertical movement including on theascending level ground, patterns of the movement including walking on the walking level ground, and ascending and descending stairs. This information can be used to filter the height estimation from descending stairs. This information can be used to filter the height estimation from the barometer to the barometer to get a better height positioning result. get a better height positioning result. TTDE is an animportant importantindicator indicator evaluate positioning accuracy. In order an accurate TTDE is to to evaluate positioning accuracy. In order to getto anget accurate TTDE, TTDE, the total travelled distance must be correctly calculated. ZUPT information is used to estimate the total travelled distance must be correctly calculated. ZUPT information is used to estimate the the average window length and this novel method is called ZUPT-based adaptive average window average window length and this novel method is called ZUPT-based adaptive average window length length (ZUPT-AAWL). methodmethod (ZUPT-AAWL). The reminder of this this paper paper is is organized organized as as follows: follows: in in Section Section 2, 2, our our MEMS-IMU-based MEMS-IMU-based The reminder of foot-mounted is described. described. In foot-mounted inertial inertial navigation navigation system system is In Section Section 3, 3, aa novel novel permanent permanent magnet-based magnet-based zero-velocity detector (mag-ZUPT) is described. In Section 4, a height difference information aided aided zero-velocity detector (mag-ZUPT) is described. In Section 4, a height difference information barometer (HDIB) algorithm is provided. In Section 5, a mag-ZUPT-based adaptive average window barometer (HDIB) algorithm is provided. In Section 5, a mag-ZUPT-based adaptive average window length is provided. provided. In trajectories are length method method (ZUPT-AAWL) (ZUPT-AAWL) is In Section Section 6, 6, experiments experiments on on different different trajectories are undertaken and the performance is verified. Finally, Section 7 concludes this work and offers some undertaken and the performance is verified. Finally, Section 7 concludes this work and offers some future research suggestions. future research suggestions. 2. System 2. MEMS-IMU MEMS-IMU Based Based Foot-Mounted Foot-Mounted Inertial Inertial Navigation Navigation System 2.1. Inertial Navigation System (INS) A foot-mounted INS uses 3-axis accelerations accelerations and angular rates to calculate the position of pedestrians. The principle of INS is that the integration of acceleration is velocity and the integration of velocity is position. The The outputs outputs of of MEMS-IMU MEMS-IMU are are in in the the sensor sensor body body coordinate coordinate frame frame (b-frame) (b-frame) and should should be betransferred transferredtotothe thenavigation navigation coordinate frame (n-frame). definition of b-frame coordinate frame (n-frame). TheThe definition of b-frame and and n-frame are shown in Figure n-frame are shown in Figure 2. 2.
(a)
(b)
Figure 2. Sketch sensor body body coordinate coordinate frame frame (b-frame) (b-frame) and and navigation navigation coordinate coordinate frame frame Figure 2. Sketch map map of of sensor (n-frame). Euler angels which areare yaw, pitch andand roll;roll; (b) shows the (n-frame). (a) (a)Shows Showsthe theb-frame b-frameand andthree three Euler angels which yaw, pitch (b) shows relation between earth-centered, earth fixed (ECEF) coordinate system and east-north-up (ENU) the relation between earth-centered, earth fixed (ECEF) coordinate system and east-north-up (ENU) coordinate coordinate system. system. ENU ENU is is better better to to be be applied applied in in our our system system than than ECEF ECEF because because that that it it has has the the same same origin with b-frame. Therefore, the rotation matrix will be simpler. origin with b-frame. Therefore, the rotation matrix will be simpler.
The b-frame is determined by the MEMS-IMU hardware and usually defined as a right-handed The b-frame is determined by the MEMS-IMU hardware and usually defined as a right-handed Cartesian coordinate system, which is shown in Figure 2a. The forward, left and upward direction Cartesian coordinate system, which is shown in Figure 2a. The forward, left and upward direction are are x, y and z axis, respectively. Considering the convenient of usage, the n-frame applied in our x, y and z axis, respectively. Considering the convenient of usage, the n-frame applied in our system is system is the local East, North, Up (ENU) Cartesian coordinate system whose origin is the same with the local East, North, Up (ENU) Cartesian coordinate system whose origin is the same with b-frame b-frame which is more intuitive and practical than ECEF or Geodetic coordinates. By convention, the which is more intuitive and practical than ECEF or Geodetic coordinates. By convention, the east axis east axis is labelled x, the north y and the up z. ECEF and ENU are depicted in Figure 2b. The is labelled x, the north y and the up z. ECEF and ENU are depicted in Figure 2b. The MEMS-IMU MEMS-IMU readings including acceleration, angular rate and magnetic field strength are in b-frame and should be transferred to ENU in order to derive the velocity and the position. The two different
Sensors 2017, 17, 2695
5 of 30
readings including acceleration, angular rate and magnetic field strength are in b-frame and should be transferred to ENU in order to derive the velocity and the position. The two different coordinate systems are transferred through the rotation matrix Cbn . It is defined from yaw, pitch and roll:
cos θ cos ψ n Cb = cos θ sin ψ − sin θ
− cos γ sin ψ + sin γ sin θ cos ψ sin γ sin ψ + cos γ sin θ cos ψ cos γ cos ψ + sin γ sin θ sin ψ − sin γ cos ψ + cos γ sin θ sin ψ sin γ cos θ cos γ cos θ
(1)
where ψ is yaw, θ is pitch and γ is roll. Cbn should be initialized using accelerometer measurements before one pedestrian starts to walk. In the rotation matrix initialization step, MEMS-IMU h remains i
stable for several seconds and the accelerations measured in the x, y and z axes of b-frame are abx , aby , abz , and the gravity of the n-frame is [0, 0, g]. Therefore, the initialization equation is:
abx 0 b n T ay = (Cb ) 0 g abz
(2)
The calculated result of Equation (2) is: ab
γ0 = arctan( yb )
θ0 = arctan( q
az abx
2 2 ( aby ) +( abz )
(3)
)
where γ0 is initial roll and θ0 is initial pitch. We can’t get initial yaw (ψ0 ) using accelerations alone. In some researches, ψ0 is manually set to a constant like 0. Magnetometer measurements are used to obtain ψ0 in this paper and the magnetometer have been calibrated to the local geomagnetic field. ψ0 = arctan(
magzb sin(γ0 ) − magyb cos(γ0 ) magbx cos(θ0 ) + magyb sin(θ0 ) sin(γ0 ) + magzb sin(θ0 ) cos(γ0 )
)
(4)
where magbx , magyb , and magzb are the x-axis, y-axis and z-axis magnetometer measurements of the b-frame, respectively. The 3-axis gyroscope makes it possible to update Cbn . Integrating the angular rates gives the relative orientation of the IMU [16]: 2I3×3 + Ω(k ) 2I3×3 − Ω(k)
(5)
− ωz ( k ) ωy ( k ) 0 −ωx (k) ωx (k) 0
(6)
Cbn (k ) = Cbn (k − 1) · where Ω(k) is:
0 Ω ( k ) = ωz ( k ) − ωy ( k )
where ωx , ωy and ωz are the x-axis, y-axis and z-axis of the b-frame gyroscope measurements, respectively. Finally, Cbn should be corrected according to the error state vector from the EKF process. It will be explained in the INS error model and the extended Kalman filter part of this paper. The 3-axis gyroscope makes it possible to calculate Cbn . Integrating the angular rates gives the relative orientation of the IMU, which can be used to transform the accelerometer measurement [18]. Euler angles, direction cosines and quaternions are the three most used rotation matrix parametrization methods. The reasons why quaternions are better to be used in navigation applications is:
Sensors 2017, 17, 2695
•
•
6 of 30
Quaternions only need to solve four equations, while direction cosines need to solve six differential equations. Therefore, the computational complexity of quaternions is smaller and this advantage is in favor of a real-time navigation system. Euler angles would bring singular values when the angles are ±90◦ . Quaternions do not have such a problem.
The quaternions were first developed by Irish mathematician William Rowan Hamilton in 1843. Quaternions are very useful to calculate three-dimensional rotations and unit quaternions can represent any rotation operation in three-dimensional space. Quaternions are a number system that extends the complex numbers and usually represented in the following form: q = q0 + q1 i + q2 j + q3 k
(7)
where q0 , q1 , q2 and q3 are real numbers. i, j and k are fundamental quaternion units. The differential equation of quaternion is: q(k ) = {cos
∆θ0 ∆θ0 [∆θ ] I+ sin } q ( k − 1) 2 ∆θ0 2
(8)
where ∆θ x , ∆θy , ∆θz and ∆θ0 are defined as:
o n ∆θ x , ∆θy , ∆θz = ωx0 (k), ωy0 (k ), ωz0 (k) · ∆t ∆θ0 =
and ∆θ is:
∆θ =
0 ∆θ x ∆θy ∆θz
q
∆θ x2 + ∆θy2 + ∆θz2
−∆θ x 0 −∆θz ∆θy
−∆θy ∆θz 0 −∆θ x
(9) (10)
−∆θz −∆θy ∆θ x 0
(11)
The rotation matrix can be transferred to the quaternion form:
q2 + q21 − q22 − q23 0 n Cb = 2(q1 q2 + q0 q3 ) 2( q1 q3 − q0 q2 )
2( q1 q2 − q0 q3 ) q20 − q21 + q22 − q23 2( q2 q3 + q0 q1 )
2( q1 q3 + q0 q2 ) 2( q2 q3 − q0 q1 ) q20 − q21 − q22 + q23
(12)
and we can calculate the four real numbers of quaternion from Cbn using:
√ 1+Cbn (1,1)+Cbn (2,2)+Cbn (3,3) q = 0 2 C n (2,3)−Cbn (3,1) q1 = b 4q0 Cbn (3,1)−Cbn (1,3) q = 2 4q0 Cbn (1,2)−Cbn (2,1) q3 = 4q0
(13)
Figure 3 shows the flowchart of the pedestrian navigation system with EKF and ZUPT. HDIB, ZUPT-AAWL and mag-ZUPT described in this paper is also shown in this figure. The pedestrian navigation system mainly contains three parts which are INS mechanization equations, EKF for error estimation and ZUPT.
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
7 of 30 7 of 30
Barometer
MEMS-IMU
posz
Accelerometer
Angular rates
Gyroscope
INS mechanization equations
posx posy
posx
Magnetometer
cali_posz
HDIB
Accelerations
Magnetic field
ZUPT-AAWL
TTDE
posy Error state vector
Permanent Magnet
EKF
Error estimation
Mid-stance detection
Stance detection
Peak-peak period calculation
Walking/run gait patterns
mag-ZUPT
Figure 3. Flowchart of the pedestrian navigation system with EKF and ZUPT. Figure 3. Flowchart of the pedestrian navigation system with EKF and ZUPT.
INS mechanization equations are a set of equations which form the basis of inertial navigation. INSgravity mechanization arefrom a setaccelerometer of equations which form thein basis of inertial Firstly, should beequations subtracted measurements n-frame. Thennavigation. position is Firstly, gravity should be subtracted from accelerometer measurements in n-frame. Then position is calculated with the gravity-free acceleration value. At last, orientation of the MEMS-IMU is updated calculated with the gravity-free acceleration value. At last, orientation of the MEMS-IMU is updated with the gyroscope measurements. These equations take slight different forms in different navigation with the[34]. gyroscope measurements. Theseaccelerometers equations take and slight different forms in different navigation frames The basic equation utilizing gyroscopes to calculate position is [17]: frames [34]. The basic equation utilizing accelerometers and gyroscopes to calculate position is [17]:
vk vk 1 (qk 1ak qk11 g )dtk 1 − g)dtk v k v k −1 + ( q k −1 a k q − k − 1 p pk 1 vk 1dtk k = pk−1 + vk−1 dtk pk = (Ω(kωdtk dt qk qk k )kq)kqk1−1
(14) (14)
ak is the where kkisisa time a time index, the gravity, is theofvelocity of the pedestrian, g is where index, g is the gravity, vk is the velocity the pedestrian, ak is the accelerometer measurements, p is the positon of the person, q is the quaternion describing the orientation frame,the dt k k of the person, is the positon is the quaternion describing accelerometer measurements, is the time differential and the quaternion (·) isdifferential orientation frame, is theΩtime and update Ω(·) is matrix. the quaternion update matrix. 2.2. 2.2. INS INS Error Error Model Model and and the the Extended Extended Kalman Kalman Filter Filter (EKF) (EKF) The a 15-element vector which is δx δr is The state statevector vectorofofthis thissystem systemis is a 15-element vector which is = [δr, = [δv, ,δϕ, ,δa, δω , ],, where ], where the is position error, error, δv is the velocity bias, δϕbias, is the attitude error, δaerror, is the accelerometer bias, δω isbias, the the position is the velocity is the attitude is the accelerometer gyroscope error. In addition, δr, δv, δϕ, δa, δω are all 3-dimension vectors. is the gyroscope error. In addition, , , , , are all 3-dimension vectors. The The state state transition transition matrix matrix FF and and G G are: are: O O t OO O O I I I I· ∆t O St · ∆t Cbnn· ∆t O O I I St t Cb t O F= O (15) O I O −Cbnn · ∆t O OO F O CO O (15) b t OI I O O OO OI I O OO
O
O
O
O
I
Sensors 2017, 17, 2695
8 of 30
G=
O Cbn · ∆t O O O
O O O O O O −Cbn · ∆t O O O I O O O I
(16)
where ∆t is the sample interval, O and I are:
0 O= 0 0
1 I= 0 0
0 0 0
0 0 0
(17)
0 1 0
0 0 1
(18)
St is the skew-symmetric matrix of acceleration:
− az (k ) ay (k ) 0 − a x (k) a x (k) 0
0 St = az (k) − ay (k )
(19)
The measurement model is: z(k) = −v(k)
(20)
where v(k ) is the velocity when ZUPT detects that the person is in stance phase: H=
h
O
I
O
O
O
i
(21)
The algorithm has the following steps: (1)
The acceleration and angular rate are compensated according to the acceleration and angular rate bias output of EKF: a0 (k) = a0 (k) + δa(k − 1) (22) ω 0 (k) = ω 0 (k) + δω (k − 1)
(23)
where a’(k) and ω’(k) are the compensated acceleration and angular rate at the time index k. a(k) and ω(k) are the original acceleration and angular rate at time index k. δa’(k−1) and δω(k−1) are the acceleration and angular rate error from EKF at time index k − 1. (2)
Update the quaternion: q(k) = {cos
∆θ0 ∆θ0 [∆θ ] I+ sin } q ( k − 1) 2 ∆θ0 2
(24)
2
where ∆θ0 = |ω 0 (k)| , ∆θ is: ∆θ =
0 ω 0 x (k) ω 0 y (k) ω 0 z (k)
−ω 0 x (k) −ω 0 y (k) −ω 0 z (k) 0 ω 0 z (k) −ω 0 y (k) −ω 0 z (k) 0 ω 0 x (k) 0 0 ω y (k) −ω x (k) 0
(25)
where ω 0 x (k), ω 0 y (k ) and ω 0 z (k) are the three components of ω 0 (k) in the x, y and z axis, respectively.
Sensors 2017, 17, 2695
(3)
9 of 30
Velocity and position update: v(k) = v(k − 1) + an (k) · ∆t
(26)
1 r (k) = r (k − 1) + v(k) · ∆t + an (k) · ∆t2 2
(27)
where k is the time index, v(k) is the velocity and r(k) is the position, an (k) is the original acceleration transferred to the navigation-frame. (4)
Correct rotation matrix Cbn . δϕ is used to compensate Cbn : 0
Cbn (k) = where δΘ is:
2I3×3 + δΘ(k) · Cbn (k) 2I3×3 − δΘ(k)
0 δΘ = δϕ(3) −δϕ(2)
−δϕ(3) δϕ(2) 0 −δϕ(1) δϕ(1) 0
(28)
(29)
where δϕ(3), δϕ(2) and δϕ(1) are the three components of δϕ from EKF which are the yaw, pitch and roll errors, respectively. Cbn (k) and Cbn0 (k ) are the attitude matrix before and after compensation. I is the 2-dimension unitary matrix. (5)
Compensate velocity and position using the error measurements from EKF: r 0 (k) = r (k) + δr (k − 1)
(30)
v0 (k) = v(k) + δv(k − 1)
(31)
where r’(k) is the compensated position and v’(k) is the compensated velocity. δr (k − 1) and δv(k − 1) are position and velocity error at time index k − 1. 3. A Novel Stance Phase Duration Model of Permanent Magnet Based ZUPT Detector (Mag-ZUPT) 3.1. Disadvantages of Threshold Based Stance Phase Detector in Running There is a basic phenomenon that when the pedestrian’s foot is totally on the ground, the velocity and angular velocity are almost zero. Considering that the positioning errors will accumulate fast due to the sensor drift, the zero-velocity information is efficient in the error correction. The detection problem is a binary hypothesis testing problem. All the ZUPT methods are based on the MEMS-IMU information including the accelerometer and gyroscope readings [22]. It can be depicted by: T = f ( a, ω ) ≤ γ
(32)
where a is the acceleration, ω is the angular rate, γ is the stance phase threshold. T is the test statistics of the detector. One typical detector is the stance hypothesis optimal detector (SHOE): 2
1 k +W − 1 1 a(k) 1 T (k) = k a(n) − g · k + 2 kw(n)k2 ≤ γ 2 W n∑ σ σ k a(k)k a w =k
(33)
where k is a time index, W is the window length, σa2 and σw2 denote the accelerometer and gyroscope noise variance, k · k is the 2-norm calculation, a(k) is the average of a during the average window W at time index k: 1 k +W − 1 a(k) = a(n) (34) W n∑ =k
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
10 of 30 10 of 30
1 k Wk W1 1 a(k )= 1 a ( n) W n a(k )= a ( n) k W nk
Sensors 2017, 17, 2695
10 of 30
(34) (34)
ThisZUPT ZUPT detector in most walking scenarios. However, this method is thresholdThis detector isisefficient efficient in most walking scenarios. However, this method is This ZUPT detector is efficient in most walking scenarios. However, this method is based and it may not be suitable when the pedestrian runs. Figure 4 shows the comparison of T threshold- based and it may not be suitable when the pedestrian runs. Figure 4 shows the comparisonin thresholdbased and it may not be suitable when the pedestrian runs. Figure 4 shows the comparison and running gait patterns. It is obvious that Tthat of aT running gait pattern is larger thanthan when ofwalking T in walking and running gait patterns. It is obvious of a running gait pattern is larger of T in walking and running gait patterns. It is obvious that T of a running gait pattern is larger than walking and the stance detector suitable for walking is not a proper choice for running. Therefore, if when walking and the stance detector suitable for walking is not a proper choice for running. when walking and the stance detector suitable for walking is not a proper choice for running. the pedestrian moves in a hybrid pattern including walking and running, there may be a wrong stance Therefore, if the pedestrian moves in a hybrid pattern including walking and running, there may be the pedestrian moves in a hybrid pattern walking and running, there may be detection ofif running which will lead to a large accumulative a Therefore, wrong stance detection of running which will lead to aincluding large error. accumulative error. a wrong stance detection of running which will lead to a large accumulative error.
T
T
T
T
Figure 4. Comparison of T in a walking and running gait pattern. Figure 4. 4. Comparison Comparison of of T T in in aa walking walking and and running running gait gait pattern. pattern. Figure
The stance phase estimation error of running gait pattern is shown in Figure 5. It is clear that the stance phase estimation of running gait pattern is shown in Figure 5. It is clear that the No. 2, The 3 and 7 stance phases are noterror detected. No. 2, 2, 33 and 7 stance phases are not detected.
Figure 5. Stance phase detection errors of running gait pattern. The black wave contains eight running Figure 5. Stance phase errors of running gait pattern. black wave gait cycles. The red line detection represents ZUPT which 1 is The the phasecontains and 0 iseight the running swing Figure 5. Stance phase detection errors of results runningingait pattern. Thestance black wave contains eight running gait cycles. The red line represents ZUPT results in which 1 is the stance phase and 0 is the swing phase. The numbers 1 to 8 are the identifier of which the gait1 is cycle. gait cycles. The red from line represents ZUPT results in the stance phase and 0 is the swing phase. phase. The numbers from 1 to 8 are the identifier of the gait cycle. The numbers from 1 to 8 are the identifier of the gait cycle.
3.2. Mid-Stance Detection Method with a Permanent Magnet Moutned on the Foot 3.2. Mid-Stance Detection Method with a Permanent Magnet Moutned on the Foot 3.2.ItMid-Stance Detection Method withstance a Permanent Magnet Moutned the Foot is important to research a new phase detector which ison more robust and can be applied It is important to research a new stance phase detector which is more robust and can bedevice applied in bothItwalking and running gait patterns. In [26] a permanent magnet is used as an additional is important to research a new stance phase detector which is more robust and can be applied in both walking and running gait patterns. In [26] a permanent magnet is used as an additional device toin have a better stance detection. One permanent detector is attached toan theadditional MEMSboth walking and phase running gait patterns. In [26] a magnetic permanent magnet is used as to have afoot better stanceaphase One permanent magnetic detector is attached to the MEMSIMU freeto to acreate local detection. magnetic field andOne from which themagnetic mid-stance can be The device have better stance phase detection. permanent detector is detected. attached to the IMU free foot to create a local magnetic field and from which the mid-stance can be detected. The MEMS-IMU free foot to create a local magnetic field and from which the mid-stance can be detected.
Sensors 2017, 17, 2695 Sensors 2017, 2017, 17, 17, 2695 2695 Sensors
11 of 30 11 of of 30 30 11
basicbasic principle is about about the relationship relationship of magnetic magnetic field strength strength and distance. distance. It is is obvious obvious that the the The principle is about the relationship of magnetic field strength and distance. It is obvious basic principle is the of field and It that magnetic field strength is stronger when the distance between magnet and sensor decrease. An that the magnetic field strength is stronger when the distance between magnet and sensor decrease. magnetic field strength is stronger when the distance between magnet and sensor decrease. An experiment was taken to analyze this phenomenon and the result is shown in Figure 6. An experiment taken to analyze phenomenon result is shown in Figure experiment waswas taken to analyze thisthis phenomenon andand the the result is shown in Figure 6. 6.
Tapemeasure measure Tape Permanent Permanent magnetshoe shoe magnet
(a) (a)
MEMS-IMUshoe shoe MEMS-IMU
Relationship of of distance distance and and magnetic magnetic field field strength strength Relationship Magnetic Magneticfield fieldstrength strength(Gs) (Gs)
Experimental scene scene Experimental
10cm 10cm
20cm 20cm 30cm 40cm 30cm 50cm 60cm 70cm 80cm 40cm 50cm 60cm 70cm 80cm
90cm 90cm
Sampling points points Sampling (b) (b)
Figure 6. 6. Relationship Relationship of distance and magnetic field strength; (a) is picture of the experiment scene. Figure Figure 6. Relationship of ofdistance distanceand andmagnetic magneticfield fieldstrength; strength; (a) (a)is isaaapicture pictureof ofthe theexperiment experimentscene. scene. The left shoe is mounted with a MEMS-IMU and the right shoe is mounted with a permanent magnet. The left left shoe shoe is is mounted mounted with with aa MEMS-IMU MEMS-IMU and and the theright rightshoe shoeisismounted mountedwith withaapermanent permanentmagnet. magnet. The right rightshoe shoemoves moves along the ruler and stops every 10(b) cm; (b) result is the theof result of the the relationship relationship The right shoe moves along ruler stops every 10 cm; (b) is result of along thethe ruler andand stops every 10 cm; is the the relationship between between distance and magnetic field strength. between and magnetic field strength. distance distance and magnetic field strength.
One of the the advantages of of applying aa permanent permanent magnet is is that the the magnetic magnetic field field strength strength is is One One of of the advantages advantages ofapplying applying a permanentmagnet magnet isthat that the magnetic field strength smoother than the acceleration and angular rate. Figures 7 and 8 show the comparison of the smoother than thethe acceleration and is smoother than acceleration andangular angularrate. rate.Figures Figures7 7and and8 8show showthe the comparison comparison of of the the waveforms of of accelerations, accelerations, angular angular rates rates and and magnetic magnetic field field strength strength in in walking walking and and running running gait gait waveforms waveforms of accelerations, angular rates and magnetic field strength in walking and running gait patterns, respectively. patterns, patterns, respectively. respectively.
Figure 7. 7. Waveforms of accelerations, accelerations, angular angular rates rates and and magnetic magnetic field field strength strength during during the the same same time time Waveforms of Figure period of walking gait pattern. of walking gait pattern. period of walking gait pattern.
Sensors 2017, Sensors 2017, 17, 17, 2695 2695
12 of of 30 30 12
Waveforms of accelerations, angular rates and magnetic field strength during the same time Figure 8. Waveforms period of of running running gait gait pattern. pattern.
Magnetic field strength (Gs)
It It is is clear clear that that the the waveform waveform of of magnetic magnetic field field strength strength contains contains less less noise noise than than acceleration acceleration and and angular rate. Furthermore, in the running gait pattern, the waveform of acceleration becomes worse angular rate. Furthermore, in the running gait pattern, the waveform of acceleration becomes worse while magnetic magnetic field fieldstrength strengthremains remainssteady. steady.The Thefluctuation fluctuationofofacceleration acceleration and angular rate leads while and angular rate leads to to wrong estimation of stance phase which is shown in Figure The stance is misjudged to wrong estimation of stance phase which is shown in Figure 9. The9.stance phasephase is misjudged to swing swing while phase,the while the mid-stance can be correctly The field magnetic fieldofstrength of the phase, mid-stance can be correctly detected. detected. The magnetic strength the permanent permanent magnet is useful to develop a better ZUPT detector. magnet is useful to develop a better ZUPT detector.
ZUPT errors errors and the magnetic field strength waveform. Figure 9. ZUPT waveform.
Sensors 2017, 17,17, 2695 Sensors 2017, 2695
13 of 13 30 of 30
Sensors 2017, 17, can 2695 be extracted from peaks of magnetic field strength waveforms which are closely 13 of 30 Mid-stances can be extracted from peaks and of magnetic field strength waveforms whichthe arenorm closely related Mid-stances with the placing relation of MEMS-IMU the permanent magnet. In this system, Mid-stances can be extracted from peaks of magnetic field strength waveforms which are closely related with the placing relation of MEMS-IMU and the permanent magnet. In this system, the norm of ( magx, magy, magz ) is used: related with the placing relation of MEMS-IMU and the permanent magnet. In this system, the norm of (magx, magy, magz) is used: of ( magx, magy, magz ) is used: 2 (35) magnorm magx magy 2 magz 2 q 2 + magy 2 2 2 + magz magnorm (35) magnorm= magx magx 2 The magy magz 2 of different magnetic (35) where magnorm is the norm of (magx,magy,magz). comparison field waveforms is shown in From this figure The we can conclude the of (magx,magy,magz) where magnorm is Figure the norm of (magx,magy,magz). The comparison ofnorm different magnetic field where magnorm is the norm of10. (magx,magy,magz). comparison ofthat different magnetic field waveforms is the best mid-stance indicator and magy has a similar performance while magx magz are not waveforms is shown Figurethis 10. figure From this we can conclude the of norm ofand (magx,magy,magz) is shown in Figure 10.inFrom wefigure can conclude that thethat norm (magx,magy,magz) is the
Magnetic field strength (Gs) Magnetic field strength (Gs)
proper choices. There are many to adetect peaks of magnorm asand utilizing the gradient is the best mid-stance indicator and has magy hasthe a performance similar performance while magx and are magz areproper not best mid-stance indicator andmethods magy similar whilesuch magx magz not of choices. theproper waveforms. The peaks are also shown in Figure 10 with red triangles. choices. many methods tothe detect the of peaks of magnorm such as utilizing the gradient There areThere manyare methods to detect peaks magnorm such as utilizing the gradient of the of the waveforms. Theare peaks also shown in Figure 10red with red triangles. waveforms. The peaks alsoare shown in Figure 10 with triangles.
Sampling points Sampling points Figure Comparison ofofdifferent magnetic field strength waveforms. The blue line, red line, green Figure 10.10. Comparison of different magnetic field strength waveforms. The blue line, red line, green Figure 10. Comparison different magnetic field strength waveforms. The blue line, red line, green and black line magx, magy, magz and magnorm, respectively. Mid-stances (peaks) from magnorm lineline and black line are magx, magy, magz and magnorm, respectively. Mid-stances (peaks) from line and black line are magx, magy, magz and magnorm, respectively. Mid-stances (peaks) from aremagnorm marked with red triangles. magnorm are marked with red red triangles. are marked with triangles.
3.3. Mag-ZUPT Method with Stance Phase Duration Model Mag-ZUPT Method with aaStance Phase Duration Model 3.3.3.3. Mag-ZUPT Method with a Stance Phase Duration Model Mid-stance be extracted from the magnetic field strength waveform ofpermanent the permanent permanent Mid-stance cancan be extracted extracted from the field strength waveform of the magnet. Mid-stance can be from themagnetic magnetic field strength waveform of the magnet. However, mid-stance is only a point of the stance phase. The sketch map of one gait cycle is However, mid-stance is only is a point the stance The sketch of map one gait cycle is cycle shown magnet. However, mid-stance only aofpoint of the phase. stance phase. The map sketch of one gait is in shown inOne Figure 11. One gait be divided into stance phaseand and swing phase. Mid-stance Mid-stance is Figure gaitOne cycle can becycle divided stance phase andphase swing phase. Mid-stance is a time point shown in11. Figure 11. gait cycle cancan be into divided into stance swing phase. is a time point when the foot is totally on the ground. whenpoint the foot is the totally ground. a time when footon is the totally on the ground.
Figure 11. Sketch map of one gait cycle. It contains stance phase and swing phase. Paper [25] only Figure Sketch map of one cycle. It contains and swing phase. Paper [25] Figure 11.11. Sketch of one gait gait cycle. It contains stancestance phase and swing phase. Paper [25] only shows how tomap detect mid-stance. We expend mid-stance tophase stance phase using stance phase only showsto how to detect mid-stance. We expend mid-stance to stance phase using stance phase duration information. shows how detect mid-stance. We expend mid-stance to stance phase using stance phase duration information. duration information.
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
14 of 30 14 of 30
An An experiment experiment was was taken taken to to have have aa more more clear clearclarification clarification of of the thecomponents components in in one one gait gaitcycle cycle which in Figure 12. T 12. is the statistics of the SHOE calculated from Equationfrom (33). whichisisshown shown in Figure T test is the test statistics of detector the SHOE detector calculated With the information of the time duration of time stanceduration phase inofone gait cycle, mid-stance caneach be Equation (33). With the information of the stance phase each in one gait cycle, expanded tocan a stance phase. According to phase. formerAccording researches,to theformer duration of the stance phase is about mid-stance be expanded to a stance researches, the duration of the 24.8%. the duration stance phase related with the different 24.8% is stance However, phase is about 24.8%.ofHowever, the is duration of stance phase ismoving relatedpatterns. with theIf different used in the running pattern, theintotal will the be smaller than thedistance reality because moving patterns. If gait 24.8% is used the travelled running distance gait pattern, total travelled will be that some swing are misjudged to stance-phase. smaller than the phases reality because that some swing phases are misjudged to stance-phase.
Figure12. 12. Relationship Relationship of of mid-stance, mid-stance, stance stance phase, phase, swing swing phase phase and andgait gaitcycle. cycle. Mid-stance Mid-stance can can be be Figure expandedtoto stance phase phase duration information. is the test statics of expanded stance phase usingusing stancestance phase duration information. T is the testTstatics of SHOE detector. SHOE detector.
The two typical types of a pedestrian indoor are walking and running. Considering that the The two typical types of a pedestrian indoor are walking and running. Considering that the length between the neighbor mid-stances is one complete gait cycle, the gait cycle length of the same length between the neighbor mid-stances is one complete gait cycle, the gait cycle length of the same person in one trajectory is directly related with the moving pattern. Figure 13 depicts the peak-peak person in one trajectory is directly related with the moving pattern. Figure 13 depicts the peak-peak time period of walking, standing still and running. This experiment is taken in a designed scene: time period of walking, standing still and running. This experiment is taken in a designed scene: (1) (1) Firstly a person walks for 140 s; (2) Secondly, the person remains stationary for 5 s; (3) At last, the Firstly a person walks for 140 s; (2) Secondly, the person remains stationary for 5 s; (3) At last, the person runs for 80 s. The statistic result is shown in Table 1. The maximum of running peak-peak time person runs for 80 s. The statistic result is shown in Table 1. The maximum of running peak-peak period is even smaller than the minimums of walking peak-peak time period. Therefore, peak-peak time period is even smaller than the minimums of walking peak-peak time period. Therefore, time period is a reliable method to distinguish walking and running gait patterns. peak-peak time period is a reliable method to distinguish walking and running gait patterns. A simple stance phase duration model is proposed. The relationship between peak-peak period A simple stance phase duration model is proposed. The relationship between peak-peak period and stance phase duration is: and stance phase duration is: 1 k + N −1 spd(k) = (36) N 1 pp ( k ) 1000N 1 nk∑ = k spd ( k ) pp ( k ) (36) 1000 nk where k is a time index, N is the window length, spdNis stance phase duration and pp is the peak-peak period. referring towindow the experiment 13 and Table 1, the and stance durations where kFor is aexample, time index, N is the length, of spdFigure is stance phase duration pp phase is the peak-peak of walking running gait patterns are 25.9%ofand 14.8%. These values are acceptable in period. For and example, referring to the experiment Figure 13 and Tabletwo 1, the stance phase durations walking/running mix trajectories. In fact, the relationship between movement patterns and stance of walking and running gait patterns are 25.9% and 14.8%. These two values are acceptable in phase duration is complex. We should consider many factors like body height and leg length. the walking/running mix trajectories. In fact, the relationship between movement patterns and In stance next paper, we will developWe a more sophisticated model using bigbody dataheight trained byleg artificial phase duration is complex. should consider many factors like and length.neural In the network. In addition, a random forest based moving pattern estimation method will also be described. next paper, we will develop a more sophisticated model using big data trained by artificial neural The flowchart of how to detect mid-stance and how to expand mid-stance to stance phase is shown network. In addition, a random forest based moving pattern estimation method will also be described. in Figure 14. The mag-ZUPT algorithm contains three steps. The first step is to extract mid-stance from The flowchart of how to detect mid-stance and how to expand mid-stance to stance phase is magnetic field waveforms. The second step is to distinguish walking/running gait patterns using shown in Figure 14. The mag-ZUPT algorithm contains three steps. The first step is to extract midpeak-peak period. Finally, mid-stance is expanded to stance with stance phase duration. gait stance from magnetic field waveforms. The second step phase is to distinguish walking/running
patterns using peak-peak period. Finally, mid-stance is expanded to stance phase with stance phase duration.
Sensors 2017, 17, 2695
15 of 30
Sensors 2017, 17, 2695
15 of 30
Table 1. Statistics of peak-peak time period of walking and running gait patterns. Sensors 2017, 17, 2695 Items
15 of 30 Peak-Peak Period of Walking Peak-Peak Period of Running Table 1. Statistics of peak-peak time period of walking and running gait patterns. Maximum 277 156 Table 1. Statistics of peak-peak Minimum 242 time period of walking and running 141 gait patterns. Items Peak-Peak Period of Walking Peak-Peak Period of Running Average 148.1 Items Peak-Peak 259.4 Period of Walking Peak-Peak Period of Running Maximum 277 156 Maximum 277 156 Minimum 242 141 Minimum 242 141 AverageAverage 259.4 148.1 259.4 148.1
Remain stationary for 5 seconds
Peak-peak period
Peak-peak period
Remain stationary for 5 seconds
Walking gait pattern Walking gait pattern
Running gait pattern Running gait pattern
Peak-peak counter Peak-peak counter
Figure 13. Using peak-peak time period information to distinguish walking/running gait pattern. Figure 13. Using peak-peak time period todistinguish distinguishwalking/running walking/running gait pattern. Figure 13. Using peak-peak time periodinformation information to gait pattern. Permanent Permanent Magnet Magnet
Magnetometer Magnetometer Magneticfield field Magnetic strength strength waveform waveform Waveform peak
Waveform peak detection detection
Mid-stance
Mid-stance
Peak-peak Peak-peak period period
Stance phase Stance model phase duration
duration model Stance phase duration Stance phase
duration
Stance phase
Stance phase Figure 14. Flowchart of mag-ZUPT. Mid-stance (peak) is extracted from magnetic field strength waveforms. Peak-peak period is used Mid-stance to distinguish(peak) walking/running gaitfrom patterns and thenfield to obtain Figure 14. Flowchart Flowchart of mag-ZUPT. mag-ZUPT. extracted magnetic strength Figure 14. of Mid-stance (peak) isis extracted stance phase duration. Finally, mid-stance is expanded to stance phase. from magnetic field strength
waveforms. Peak-peak Peak-peak period period is is used used to to distinguish distinguish walking/running walking/running gait gait patterns patterns and and then then to to obtain obtain waveforms. stance phase duration. Finally, mid-stance is expanded to stance phase. stanceLike phase duration. Finally, mid-stance is expanded to stance other ZUPT detectors, stance phase is represented as 1 phase. and swing phase is represented as 0 to compose mag-ZUPT. The results of traditional ZUPT and mag-ZUPT is shown in Figure 15. The two ZUPT have a similar in walking.as to thephase running gait pattern, as Like other detectors ZUPT detectors, detectors, stanceperformance phase is is represented represented asWhile andasswing swing phase is represented represented as 00 Like other ZUPT stance phase 11 and is seven stance phases The are by mag-ZUPT compared with only one phase is The to compose mag-ZUPT. results ZUPT and mag-ZUPT is shown instance Figure 15. The two to mag-ZUPT. Thecorrectly resultsofdetected oftraditional traditional ZUPT and mag-ZUPT is shown in Figure 15.
ZUPT detectors havehave a similar performance in walking. WhileWhile as to the running gait pattern, seven two ZUPT detectors a similar performance in walking. as to the running gait pattern, seven stance phases are correctly detected by mag-ZUPT compared with only one stance phase is
Sensors 2017, 17, 2695 Sensors2017, 2017,17, 17,2695 2695 Sensors
16 of 30 1616ofof3030
detected by by traditional traditional ZUPT. Therefore, Therefore, mag-ZUPT mag-ZUPT has has a a better better performance performance especially especially inin detected stance phases are correctlyZUPT. detected by mag-ZUPT compared with only one stance phase is detected runningscenarios. scenarios. running by traditional ZUPT. Therefore, mag-ZUPT has a better performance especially in running scenarios. 2 2
2 2
Traditional ZUPT Traditional ZUPT mag-ZUPT mag-ZUPT
1.51.5
1 1
1 1
0.50.5
0.50.5
0 0
0 0
-0.5-0.5
-0.5-0.5
-1 -1
1.21.2
1.22 1.22
1.24 1.24
1.26 1.26
1.28 1.28
1.31.3
-1 -1
1.32 1.32 10 410
Traditional ZUPT Traditional ZUPT mag-ZUPT mag-ZUPT
1.51.5
4
3.89 3.93.9 3.91 3.91 3.92 3.92 3.93 3.93 3.94 3.94 3.95 3.95 3.96 3.96 3.97 3.97 3.98 3.98 3.89 10 410
4
Figure 15. Traditional ZUPT and mag-ZUPT results ininwalking walking and running gait patterns; (a) isisthe the Figure15. 15.Traditional TraditionalZUPT ZUPTand andmag-ZUPT mag-ZUPTresults resultsin walkingand andrunning runninggait gaitpatterns; patterns;(a) (a)is the Figure walking scenario. The two ZUPT detectors have a similar performance; (b) is the running scenario. walkingscenario. scenario.The Thetwo twoZUPT ZUPTdetectors detectorshave havea asimilar similarperformance; performance;(b) (b)isisthe therunning runningscenario. scenario. walking Seven stance phases are correctly detected by mag-ZUPT while only one stance phase isisdetected by Sevenstance stancephases phasesare arecorrectly correctlydetected detectedby bymag-ZUPT mag-ZUPTwhile whileonly onlyone onestance stancephase phaseis detectedby by Seven detected traditional ZUPT. traditionalZUPT. ZUPT. traditional
Anexperiment experimentof awalking/running walking/runningmix mix trajectory was undertaken toverify verify the performance trajectory was undertaken totoverify thethe performance of An experiment ofofaawalking/running mix trajectory was undertaken performance of the mag-ZUPT method. The trajectory is a closed path that the first half of the trajectory is walking the mag-ZUPT method. The trajectory is a closed path that the first half of the trajectory is walking of the mag-ZUPT method. The trajectory is a closed path that the first half of the trajectory andthen thenthe thepedestrian pedestrianran ranback backtotothe thestart startpoint. point.The Thepositioning positioningresults resultsusing usingmag-ZUPT mag-ZUPTand and and start point. traditionalZUPT ZUPTare areshown shownininFigure Figure16. 16.ItItisisobvious obviousthat thatthe thewalking walkingtrajectory trajectory(first (firsthalf) half)ofofthese these traditional twomethods methodsare aresimilar. similar.However, However,the therunning runningtrajectory trajectoryusing usingtraditional traditionalZUPT ZUPTdiverges divergesrapidly rapidly methods two becauseofofthe thestance stancephase phaseestimation estimationerrors. errors. because
Figure 16. Positioning results ofofaa awalking/running mix trajectory. The first half path isiswalking walking and Figure16. 16.Positioning Positioningresults resultsof walking/runningmix mixtrajectory. trajectory.The Thefirst firsthalf halfpath pathis walkingand and Figure walking/running the second half path is running. Traditional ZUPT cannot detect stance phase correctly of running gait thesecond secondhalf halfpath pathisisrunning. running.Traditional TraditionalZUPT ZUPTcannot cannotdetect detectstance stancephase phasecorrectly correctlyofofrunning running the pattern, therefore the positioning result result diverges rapidly. gaitpattern, pattern, therefore thepositioning positioning diverges rapidly. gait therefore the result diverges rapidly.
4.4.Height Height Difference Information Aided Barometer (HDIB) Algorithm HeightDifference DifferenceInformation InformationAided AidedBarometer Barometer(HDIB) (HDIB)Algorithm Algorithm 4. Considering only aafew meters can lead totoan error ofof several floors, thethe demand of Consideringthat thataa abias biasof only afew few meters can lead toan an error ofseveral several floors, the demand Considering that bias ofof only meters can lead error floors, demand height accuracy is even higher than the level ground scenario. However, the accelerometer and the of height accuracy is even higher than the level ground scenario. However, the accelerometer and the of height accuracy is even higher than the level ground scenario. However, the accelerometer and the gyroscope to obtain aapoor height estimation. As totothe barometer, it is to gyroscopedrifts driftsare areaccumulated accumulated obtain apoor poor height estimation. As tothe the barometer, easy gyroscope drifts are accumulated toto obtain height estimation. As barometer, ititeasy isiseasy to be influenced by the environment and this phenomenon is shown in Figure 17. An experiment was to be influenced by the environment and this phenomenon is shown in Figure 17. An experiment was
Sensors 2017, 17, 2695
17 of 30
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
17 of 30 17 of 30
Height(m) (m) Height
atmosphericpressure pressure(Pa) (Pa) atmospheric
be influenced by the environment andthe this phenomenon is shown in Figure 17. An experiment was taken taken that that the the pedestrian pedestrian walked walked on on the level level ground ground for for 100 100 ss and and itit is is obvious obvious that that even even on on the the level level taken that the pedestrian walked on the level ground for 100 s and it is obvious that even on the level ground, ground, the the atmospheric atmospheric pressure pressure fluctuates fluctuates with with the the pedestrian pedestrian movement movement which which leads leads to to height height ground, the atmospheric pressure fluctuates with the pedestrian movement which leads to height estimation estimation errors. errors. estimation errors.
Time Time (s) (s) (a) (a)
True vertial postion True vertial postion
Time Time (s) (s) (b) (b)
Figure 17. The atmospheric pressure fluctuates when the pedestrian walks on the same height; (a) Figure 17. The atmospheric fluctuates when the pedestrian walks the same height; (a) isis Figure 17. The atmosphericpressure pressure fluctuates when the pedestrian walkson on the same height; (a) is the atmospheric pressure; (b) is the height estimation error caused by atmospheric pressure the atmospheric pressure; (b) is the height estimation error caused by atmospheric pressure fluctuation. the atmospheric pressure; (b) is the height estimation error caused by atmospheric pressure fluctuation. line in true The red line inThe (b)red is the true vertical (0 m).position fluctuation. The red line in (b) (b) is is the theposition true vertical vertical position (0 (0 m). m).
In order achieve high accuracy height estimation our 3D positioning system, novel ordertoto toachieve achievea aahigh highaccuracy accuracyheight heightestimation estimationinin inour our3D 3Dpositioning positioningsystem, system,aaanovel novel InInorder height difference information aided barometer (HDIB) algorithm developed. HDIB utilizes both heightdifference differenceinformation informationaided aidedbarometer barometer(HDIB) (HDIB)algorithm algorithmisis isdeveloped. developed.HDIB HDIButilizes utilizesboth both height MEMS-IMU and barometer readings. It is dependent on the basic thinking that normally the height MEMS-IMU and barometer readings. It is dependent on the basic thinking that normally the height MEMS-IMU and barometer readings. It is dependent on the basic thinking that normally the height motion person determined by the ground mode which includes level ground, upstairs and motionofof ofa aaperson personisis isdetermined determinedby bythe theground groundmode modewhich whichincludes includeslevel levelground, ground,upstairs upstairsand and motion downstairs. When walking on a level ground, the height is invariant. While walking up or down, the downstairs.When Whenwalking walkingon on aa level ground, the upup oror down, the downstairs. the height heightisisinvariant. invariant.While Whilewalking walking down, height estimation should be updated. Luckily, MEMS-IMU has the ability to estimate the ground height estimation should be updated. Luckily, MEMS-IMU has the ability to estimate the ground the height estimation should be updated. Luckily, MEMS-IMU ability to estimate the ground mode precisely because although the error accumulated, the short-time results of IMU are accurate. modeprecisely preciselybecause becausealthough althoughthe theerror erroraccumulated, accumulated,the theshort-time short-timeresults resultsof ofIMU IMUare areaccurate. accurate. mode The accumulative error of height estimation is shown in Figure 18. We can see that The accumulative of height estimation in We Figure 18. that Weonly can5000 see sampling that only only The accumulative errorerror of height estimation is shownisinshown Figure 18. can see 5000 sampling points later (25 s), the height error is 22 m which is unacceptable. However, we can see 5000 sampling points later (25 s), the height error is m which is unacceptable. However, we can see points later (25 s), the height error is 2 m which is unacceptable. However, we can see that, in Figure 18b, that, in Figure 18b, the maximum height difference every one second is 0.11 m. It stays steady and is that, in Figure 18b, the maximum height difference every second is 0.11 stays steady the maximum height difference every one second is 0.11 m. one It stays steady andm. is It possible to useand thisis possible to use this information to aid barometer to estimate the height more precisely. possible to to use this information to aid barometer estimate the height more precisely. information aid barometer to estimate the heightto more precisely.
Figure 18. The accumulative error of height estimation andand the height difference every one second; (a) Figure Figure 18. 18. The The accumulative accumulative error error of of height height estimation estimation and the the height height difference difference every every one one second; second; is(a) the accumulative error. It reaches 2 m after 5000 samplingsampling points (25 s); (b) (25 is the height difference (a) is is the the accumulative accumulative error. error. ItIt reaches reaches 22 m m after after 5000 5000 sampling points points (25 s); s); (b) (b) is is the the height height every one second. Although the height error accumulated, the height difference stay steady and it is difference differenceevery everyone onesecond. second.Although Althoughthe theheight heighterror erroraccumulated, accumulated,the theheight heightdifference differencestay staysteady steady possible to use thisto information to aid barometer to estimate the height more precisely. precisely. and and itit is is possible possible to use use this this information information to to aid aid barometer barometer to to estimate estimate the the height height more more precisely.
The flowchart HDIB shown Figure 19. has two stages. The first stage distinguish The Theflowchart flowchartofof ofHDIB HDIBisis isshown showninin inFigure Figure19. 19.ItItIthas hastwo twostages. stages.The Thefirst firststage stageisis istoto todistinguish distinguish level ground/upstairs/downstairs with MEMS-IMU readings. The second stage is to adaptively set level ground/upstairs/downstairs with MEMS-IMU readings. The second stage is to adaptively set the level ground/upstairs/downstairs with MEMS-IMU readings. The second stage is to adaptively set the reference atmospheric pressure according to the result of the first stage. Then difference between the reference atmospheric pressure according to the result of the first stage. Then difference between
Sensors 2017, 17, 2695
18 of 30
Sensors 2017, 17, 2695
18 of 30
reference atmospheric pressure according to the result of the first stage. Then difference between the the measured atmospheric pressure the reference atmospheric pressure is used to calculate measured atmospheric pressure and theand reference atmospheric pressure is used to calculate pedestrians’ pedestrians’ vertical position. vertical position.
INS
Height difference calculation
Height stays constant
YES
Level ground NO
Update reference atmospheric pressure
Get barometer readings
Height calculation
Figure 19. 19. Flowchart Flowchart of of height height difference difference information information aided aided barometer barometer (HDIB) (HDIB) algorithm. algorithm. Figure
We can get accurate height difference between contiguous steps using MEMS-IMU. MEMS-IMU. It can be used to walking onon thethe level ground, walking upstairs or downstairs. The to judge judgewhether whethera aperson personis is walking level ground, walking upstairs or downstairs. equations of ground mode estimation are: The equations of ground mode estimation are: 1 kw w kw+w h 1 IMUhn k h ∑ I MUhn kw= w n kwn 1=kw +1 hd h h h −h hd =
k
k
k
k 1k
k −1
α < hd < β → HDF = f lat hdkk HDFkk flat hdk < α → HDFk = down hd k HDFk down hdk > β → HDFk = up
(37) (37)
hd k HDFk up
where IMUh is the height estimation result from INS, w is the window length for the average calculation, h is the mean height calculated from the IMU system, hd is the height difference, k is the time index, α is the height result fromβINS, the window length for thenumber. average where IMUhthreshold is the down whichestimation is a negative number, is thewupisthreshold which is a positive calculation, h isofthe mean height calculated the IMU system, hd isNormally the height k is the The value w is based on the samplingfrom frequency of MEMS-IMU. w difference, = 1/f is acceptable. time index, α is the down threshold which is a negative number, β is the up threshold which is a Considering that a person’s leg raises 20 cm during a gait cycle and the average time is set to one positive number. second, α is set to −0.1 m and β is set to +0.1 m. The w is based on the sampling frequency MEMS-IMU. Normally w = 1/f is acceptable. The value secondofstage is to calculate pedestrians’ verticalofposition [30]. The ideal atmospheric equation Considering that a person’s leg raises 20 cm during a gait cycle and the average time is set to one is: second, α is set to −0.1 m and β is set to +0.1 m. P = ρRd T (38) The second stage is to calculate pedestrians’ vertical position [30]. The ideal atmospheric where P is: is the atmospheric pressure, ρ is the air density, Rd is the gas constant whose value is equation 287.05287 m2 /(s2 K), T is the thermodynamic temperature of the air. According to the atmospheric P pressure Rd T and height is: (38) static equation, the relationship of atmospheric where P is the atmospheric pressure, ρ is the air density, Rd is the gas constant whose value is dP = −ρgdH (39) 287.05287 m2/(s2K), T is the thermodynamic temperature of the air. According to the atmospheric static equation, the relationship of atmospheric pressure and height is:
dP gdH
(39)
Sensors 2017, 17, 2695
19 of 30
Sensors where2017, g is 17, the2695 local
gravity,
(·) is the differential calculation, H the height. Take (38) to (39): 19 of 30
dP
g
calculation, dH H the height. Take (38) to (39): where g is the local gravity, d(·) is the differential P RT
(40)
d
g from vertical position Hr to Hm, Equation (40) dP Ignoring the variation of gravity and temperature, =− dH (40) P R dT can be integrated: Pm temperature, Hm Ignoring the variation of gravity and dP gfrom vertical position Hr to Hm , Equation (40) dH can be integrated: (41)
ZPPrm
P
RT
ZHm d H
r dP g =− dH (41) P Hm is the Rd Tvertical position needed to be measured, where Hr is the reference vertical position, Pr Hr Pr is the atmospheric pressure of Hr, Pm is the atmospheric pressure of Hm. The calculation result of where Hr (41) is the Equation is: reference vertical position, Hm is the vertical position needed to be measured, Pr is the atmospheric pressure of Hr , Pm is the atmospheric pressure of Hm . The calculation result of RT P Equation (41) is: H m d k ln r H r (42) Rd g Tk Pmr Hm = ln + Hr (42) g Pm where Tk is a simplified temperature integration result of Equation (41) which is an average value of where Tk is a simplified temperature integration of Equation (41) which is an average value of Tr Tr (temperature of Hr) and Tm (temperature of Hresult m): (temperature of Hr ) and Tm (temperature of Hm ):
T T Tk Trr + Tmm 2 Tk =
(43) (43)
2 Hr is adaptively adjusted to the result from stage 1 of HDIB. An experiment was taken to test HrThe is adaptively to the from stage 1 of HDIB.up An experiment taken to four test HDIB. pedestrianadjusted started from theresult second floor, then walked four floors andwas went down HDIB. The pedestrian started from theexperiment second floor, then walked up four floors down four floors to return to the start point. This was taken in a shopping mall and and went the total vertical floors to return to the start point. This experiment was taken in a shopping mall and the total vertical moving height is around 24 m. The height of start point and end point is 0 m. The sequence of walking moving height around 24 m. height start point andflat, enddown, point is 0 m. Theand sequence of is flat, up, flat, isup, flat, up, flat,The down, flat,ofup, flat, down, flat, down flat. The walking is flat, up,offlat, up,ground/upstairs/downstairs flat, up, flat, down, flat, up, is flat, down,inflat, down, flat, it down and flat. estimation result level shown Figure 20 and matches withThe the estimation result of level ground/upstairs/downstairs is shown in Figure 20 and it matches with the reference height (true height). The positioning results are shown in Figure 21. Three different reference (true height). results are shown in Figure Three different methods methods height are tested which The are positioning MEMS-IMU, barometer and HDIB.21.Height estimation from are tested which areworst MEMS-IMU, and HDIB.error. Height estimation MEMS-IMU the MEMS-IMU is the because barometer of the accumulative The result offrom using barometer is alone worst because of the accumulative error. The result of using barometer alone fluctuates because of the fluctuates because of the instability of barometer outputs and the environment. HDIB performs well instability barometer and methods. the environment. HDIB performserror well of comparing the two comparingofwith the twooutputs traditional The height estimation the three with methods are traditional methods. The height estimation error of the three methods are listed in Table 2. listed in Table 2.
Height difference (m)
U
LG
U
LG
U
U
LG
LG
LG
D
LG
LG
D
LG
D
LG
D
Time (s) Figure estimation results. Figure 20. 20. Height Height difference differenceand andthe thelevel levelground/upstairs/downstairs ground/upstairs/downstairs estimation results. LG LG is is short short for level ground. U is short for upstairs. D is short for downstairs. for level ground. U is short for upstairs. D is short for downstairs.
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
20 of 30 20 of 30
Sensors 2017, 17, 2695 Table 2. Height estimation result of MEMS-IMU, Barometer and HDIB.
20 of 30
Height (m)
Height (m)
Table 2. Height estimation result of MEMS-IMU, Barometer and HDIB. Method & End (m) result Peak (m) Final Height (m) and Height Error (m) Table 2.Start Height estimation of MEMS-IMU, Barometer HDIB. MEMS-IMU 0 24 51.72 51.72 Method & End (m)(m) Peak Peak(m) (m) Final Final Height (m) Height (m) Error (m) Method Start Start & End Height Barometer 0 24 5.98 (m) Height Error 5.98 MEMS-IMU 0 2424 51.7251.72 51.72 51.72 MEMS-IMU 0 HDIB 0 24 2.04 2.04 Barometer 0 0 Barometer 2424 5.98 5.98 5.98 5.98 HDIBHDIB 0 0 2424 2.04 2.04 2.04 2.04
Time (s) Time (s) Figure 21. Height estimation using MEMS-IMU, barometer and HDIB. The blue line, red line and Figure 21.21. Height estimation using MEMS-IMU, barometer and HDIB. The blue line, redred lineline andand black Figure Height estimation using MEMS-IMU, barometer and HDIB. The blue line, black line are height estimation from MEMS-IMU, barometer and HDIB, respectively. The green line line are line height from MEMS-IMU, barometer and HDIB, respectively. TheThe green lineline is the black areestimation height estimation from MEMS-IMU, barometer and HDIB, respectively. green is the reference height (true height). reference height (true height). is the reference height (true height).
5. A Mag-ZUPT-Based Adaptive Average Window Length Method (ZUPT-AAWL) Mag-ZUPT-BasedAdaptive AdaptiveAverage AverageWindow Window Length Method 5. 5. AA Mag-ZUPT-Based Method(ZUPT-AAWL) (ZUPT-AAWL) TTDE is an important indicator to evaluate the performance of theinertial inertialnavigation navigation system. TTDEisisananimportant importantindicator indicator to to evaluate evaluate the system. TTDE the performance performanceofofthe the inertial navigation system. TTDE is defined as the percentage error of the total travelled distance in a closed-loop trajectory: TTDE is defined as the percentage error of the total travelled distance in a closed-loop trajectory: TTDE is defined as the percentage error of the total travelled distance in a closed-loop trajectory:
pp((start end)) start)) pp((end TTDE TTDE k p(start) − p(end)k TTDE = L L L
(44) (44) (44)
where p(end) and p(start) the positioning results, 2-norm where p(end) and p(start)are arethe theend endpoint pointand andstart start point the 2-norm where p(end) and p(start) are the end point and start pointof ofthe thepositioning positioningresults, results, k· is k is isthe the 2-norm calculation, L is the total travelled distance. calculation, L is the total calculation, L is the totaltravelled travelleddistance. distance. Considering that the MEMS-IMU sampling frequency is very high, the original positioning results Considering that the MEMS-IMU sampling frequency is high, the positioning Considering that the MEMS-IMU sampling frequency is very very high, theoriginal original positioning results trajectory ofthe theMEMS-IMU MEMS-IMU mounted foot which isisnot same with depict thedepict trajectory of the of MEMS-IMU mounted foot (IMU-foot), which is not the same with the results depict thethe trajectory mounted foot (IMU-foot), (IMU-foot), which notthe the same with the walking trajectory. Traditionally, the original positioning results are averaged in a fix length like walking trajectory. Traditionally, the original positioning results are averaged in a fix length like 1s the walking trajectory. Traditionally, the original positioning results are averaged in a fix length like 1 s because that the gait cycle duration is around 1 s. There is a problem that the averaged results are thatthat thethe gaitgait cycle duration is around 1 s.1There is aisproblem thatthat thethe averaged results are are not 1because s because cycle duration is around s. There a problem averaged results not one complete gaitcycle. cycle. may beaalarger little larger larger or smaller than gait cycle to to thethe one complete gait gait cycle. It may a little or smaller than the gaitthe cycle according to the length of not one complete ItItbe may be little or smaller than the gait cycleaccording according length of the average window. the average length of thewindow. average window. is shown Figure 22, duringevery everygait gaitcycle, cycle,there there must that the IMU-foot onthe AsAs is shown inin Figure 22, during bebe a atime that the IMU-foot isison As is Figure 22, during every gait cycle, theremust must be atime time that the IMU-foot is on the ground. It is obvious that , , , and are better choices to be used as the average ground. It is obvious that k k , k k , k k , and k k are better choices to be used as the average window the ground. It is obvious that ,4and are better choices to be used as the average 5 1 2 2 3, 3 4 window length. Because that the length of every gait cycle is different, this window length is adaptive length. Because that the that length everyofgait cycle different, this window lengthlength is adaptive to the window length. Because theof length every gaitiscycle is different, this window is adaptive to the specific gait cycle. The flowchart of ZUPT-AAWL algorithm is shown in Figure 23. specific gait cycle. The flowchart of ZUPT-AAWL algorithm is shown in Figure 23. to the specific gait cycle. The flowchart of ZUPT-AAWL algorithm is shown in Figure 23.
Figure 22. The sketch map of the trajectory of IMU-foot. There are four gait cycles in this figure. Red Figure map trajectory of There are dots 22. represent that IMU-foot is on the ground which is the mid-stance. Figure 22. The Thesketch sketch mapof ofthe the trajectory of IMU-foot. IMU-foot. There arefour fourgait gait cycles cyclesin in this this figure. figure. Red Red dots dotsrepresent represent that that IMU-foot IMU-foot is is on on the the ground ground which which is the mid-stance.
Sensors 2017, Sensors 2017, 17, 17, 2695 2695
21 of of 30 30 21
Start & end point of stance phase
ZUPT information
Calculate the average window length Positioning results
kth traveled distance
Sum
Total traveled distance Figure 23. Flowchart of ZUPT-AAWL. Figure23. Flowchart of ZUPT-AAWL.
Start and and end end point point of of stance stance phase phase information information from from ZUPT ZUPT can can be be used used to to determine determine the the average average Start window length: window length: winlen(k) = stillk (end) − stillk (start) (45) winlen(k ) stillk (end ) stillk ( start ) (45) where winlen(k) is the k-th average window length of stance phase, stillk (end) is the end point of the k-th where the (start) k-th average window stance phase, stillTherefore, k(end) is the end point of the kstance winlen(k) phase andisstill is the start pointlength of theof k-th stance phase. the value of average k th stance phase and still k(start) is the start point of the k-th stance phase. Therefore, the value of window length is represented in sampling points and has no unit. average lengthwill is represented inwith sampling points and haswinlen(k): no unit. Thewindow k-th trajectory be averaged the window length The k-th trajectory will be averaged with the window length winlen(k): winlen(k )
winlen ( k ) 11 trajsmooth (k()k= traj(ki()i ) ) winlen(k) × ∑ traj trajsmooth k i =1
winlen(k )
(46) (46)
i 1
where length of the k-thk-th stance phase, trajk traj is the of the of k-th stance where trajsmooth(k) trajsmooth(k)isisthe theaverage average length of the stance phase, k istrajectory the trajectory the k-th phase. The trajectory length length is calculated using: using: stance phase. The trajectory is calculated
trajlen((kk)) = trajlen
q
2
2
((trajsmooth ( k(k− 1))1))2(+ trajsmooth ( k )y (ktrajsmooth ( k y (1)) trajsmoothx x((kk))−trajsmooth trajsmooth (trajsmooth ) − trajsmooth k − 1))2 (47) (47) x x y y
trajectory length, trajsmoothxx and where trajlen(k) is the k-th trajectory and trajsmooth trajsmoothyy are are the the xx and and y values of trajsmooth, respectively. respectively. All the trajectory lengths are summed to calculate the total travelled distance: distance:
LL = trajlen trajlen((kk))
∑
(48) (48)
An experiment was performed to verify ZUPT-AAWL. The real total travelled distance An experiment was performed to verify ZUPT-AAWL. The real total travelled distance measured measured by a laser range-finder is 1355 m. The relationship of total travelled distance and average by a laser range-finder is 1355 m. The relationship of total travelled distance and average window window length is shown in Figure 24. The total travelled distance is decreased with the increase of length is shown in Figure 24. The total travelled distance is decreased with the increase of average average window length. The red dots are the range of ZUPT-AAWL average window lengths. It window length. The red dots are the range of ZUPT-AAWL average window lengths. It ranges from ranges from 20 to 381 sampling points according to different gait cycles. The total travelled distance 20 to 381 sampling points according to different gait cycles. The total travelled distance results of four results of four typical fix window length including 1, 100, 200, 300, 400 sampling points and typical fix window length including 1, 100, 200, 300, 400 sampling points and ZUPT-AAWL are shown ZUPT-AAWL are shown in Table 3. The result using ZUPT-AAWL is closest to the real length. In in Table 3. The result using ZUPT-AAWL is closest to the real length. In addition, ZUPT-AAWL is a addition, ZUPT-AAWL is a more universal method that is adaptive to different trajectories, different more universal method that is adaptive to different trajectories, different moving patterns and different moving patterns and different gait cycles. gait cycles.
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
Total travelled distance (m)
22 of 30 22 of 30
Average window length (in sampling points)
Figure Figure 24. 24. Relationship Relationshipof oftotal totaltravelled travelled distance distance and and average average window window length. length. The Thered red dots dots are are range range of ZUPT-AAWL ZUPT-AAWL average window length which ranges from 20 to 381 sampling of sampling points. points Table Table 3. 3. Typical Typical total travelled distance results. Average Window Length Average Window Length Real length Real length 1 1100 100 200 200 300 300 400 400 ZUPT-AAWL ZUPT-AAWL
Total Travelled Distance (m) Total Travelled Distance (m) 1355 1612 1355 1394 1612 1394 1353 1353 1342 1342 1332 1332 1356 1356
6. Experimental Results 6. Experimental Results 6.1. 6.1. Experimental Experimental Setup Setup In In order order to to evaluate evaluate the the performance performance of of the the 3D 3D positioning positioning system, system, trajectories trajectories of of multiple multiple scenarios are tested. The MEMS-IMU used in our system is an AHRS-1 (Inertial Labs, scenarios are tested. The MEMS-IMU used in our system is an AHRS-1 (Inertial Labs, Paeonian Paeonian Springs, VA,USA), USA), which contains a accelerometer, 3-axis accelerometer, a 3-axis and gyroscope and a 3-axis Springs, VA, which contains a 3-axis a 3-axis gyroscope a 3-axis magnetometer. magnetometer. The MEMS-IMU is mounted on the heel of the left shoe and one permanent magnet The MEMS-IMU is mounted on the heel of the left shoe and one permanent magnet is mounted on is mounted on the right shoe. The diameter and thickness of the permanent magnet used the right shoe. The diameter and thickness of the permanent magnet used in the experiment is in 2.5the cm experiment is 2.5 cm and 1 cm, respectively. A processing mounted on the aleft shoe. and 1 cm, respectively. A processing board is also mounted board on the is leftalso shoe. Furthermore, BMP280 Furthermore, a BMP280 barometer (Bosch, Stuttgart, Baden-Württemberg, Germany) is installed on barometer (Bosch, Stuttgart, Baden-Württemberg, Germany) is installed on the processing board. the board. The raw data and results processed saved inprocessing the storageboard. card Theprocessing raw data and processed positioning are positioning saved in theresults storageare card of the of the processing board. The 9-axis data obtained from the sensor is processed on board to get the The 9-axis data obtained from the sensor is processed on board to get the pedestrian positioning results pedestrian positioning results and then transmitted to the server in real-time. In order to evaluate the and then transmitted to the server in real-time. In order to evaluate the performance more carefully, in performance more inisthis paper, the 9-axis dataplatform. is processed the MATLAB this paper, the raw carefully, 9-axis data processed on raw the MATLAB The on system hardwareplatform. structure The system hardware structure is shown in Figure 25. is shown in Figure 25. The to have a full theofsystem. Firstly, Firstly, three walking gait pattern, Theexperiments experimentsare aredesigned designed to have a test full of test the system. three walking gait three running gait pattern and three walking/running mixed trajectories are taken to test the magpattern, three running gait pattern and three walking/running mixed trajectories are taken to test the ZUPT performance. The evaluation indicator of thisof experiment is the accuracy rate of rate stance phase mag-ZUPT performance. The evaluation indicator this experiment is the accuracy of stance estimation. Secondly, nine different ascending/descending stairs experiments are taken to test phase estimation. Secondly, nine different ascending/descending stairs experiments are taken HDIB to test algorithm. Finally, six 3D trajectories are taken to evaluate the general performance of this system. In HDIB algorithm. Finally, six 3D trajectories are taken to evaluate the general performance of this order to get an accurate TTDE, ZUPT-AAWL is used to calculate the total walking distance. system. In order to get an accurate TTDE, ZUPT-AAWL is used to calculate the total walking distance.
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
23 of 30
23 of 30 Barometer
MEMS-IMU Accelerometer Gyroscope
Processing Board
Magnetometer
Permanent Magnet
Positioning Results
Application Server Antenna
Storage Card 9-axis Raw Data Barometer Raw Data
Fire Rescue Tunnel Positoning ……
Figure 25. Hardware the 3D positioning Figure 25. Hardware structurestructure of the 3Dofpositioning system. system.
6.2. Mag-ZUPT Experiments 6.2. Mag-ZUPT Experiments Nine areontaken on the 100 m runway of a stadium. Three walking gait pattern, three running Nine tests aretests taken the 100 m runway of a stadium. Three walking gait pattern, three running gait pattern and three walking/running mix trajectories are taken to test the mag-ZUPT performance. gait pattern and three walking/running mix trajectories are taken to test the mag-ZUPT performance. truephase stancenumber phase number is countered by arecording tester recording theStance steps. phases Stance phases of SHOE The trueThe stance is countered by a tester the steps. of SHOE (shown in Equation (33)) and mag-ZUPT are calculated from the number of 1 in ZUPT. (shown in Equation (33)) and mag-ZUPT are calculated from the number of 1 in ZUPT. The of results of the nine areinlisted 4. phase Stanceestimation phase estimation performance of walking The results the nine tests aretests listed Tablein4.Table Stance performance of walking gait pattern is similar thatlow in the low dynamic movement, acceleration and angular gait pattern is similar becausebecause that in the dynamic movement, acceleration and angular rate arerate are relatively stable. As to the running gait pattern, the stance phase estimation error of SHOE is serious relatively stable. As to the running gait pattern, the stance phase estimation error of SHOE is serious the threshold is adjusted to walking, while mag-ZUPT method has good performance. becausebecause that the that threshold is adjusted to walking, while mag-ZUPT method has good performance. In In the walking/running tests, the subject walks and runs for several steps in turn. Considering that the walking/running tests, the subject walks and runs for several steps in turn. Considering that there there are some walking steps, stance phase estimation of SHOE is better than the running tests. are some walking steps, stance phase estimation of SHOE is better than the running tests. The average number the nine is 50.3. average error SHOE and magThe average stance stance phase phase number of theofnine tests tests is 50.3. TheThe average error of of SHOE and ZUPT is 13.9/50.3 and 0.3/50.3, respectively. The mag-ZUPT method can get a much better mag-ZUPT is 13.9/50.3 and 0.3/50.3, respectively. The mag-ZUPT method can get a much better stance stance phase estimation accuracy andestimation ZUPT estimation in multiple phase estimation accuracy and ZUPT in multiple movingmoving patterns.patterns. Stance phase estimation SHOE and mag-ZUPT. Table 4. Table Stance4.phase estimation accuracyaccuracy of SHOEofand mag-ZUPT. Trajectory Stance Phase Number Stance Phase Number of SHOE Stance Phase Number of Mag-ZUPT Trajectory Stance Phase Number Stance Phase Number of SHOE Stance Phase Number of Mag-ZUPT Walking_1 70 69 70 Walking_1 70 69 70 Walking_2 68 69 68 Walking_2 68 69 68 Walking_3 67 66 67 Walking_3 67 66 67 Running_1 39 14 39 Running_1 39 14 39 Running_2 38 14 37 Running_2 38 14 37 Running_3 37 4 36 Running_3 37 4 36 45 34 45 Mix_1 Mix_1 45 34 45 Mix_2 Mix_2 45 32 44 45 32 44 Mix_3 Mix_3 44 28 44 44 28 44 Average Average error None None 13.9/50.3 0.3/50.30.3/50.3 error 13.9/50.3
Furthermore, the mistake of phase stanceestimation phase estimation to the positioning result divergence. Furthermore, the mistake of stance leads toleads the positioning result divergence. Table 5 shows the total travelled distance of SHOE and mag-ZUPT. It is closely connected Table 5 shows the total travelled distance of SHOE and mag-ZUPT. It is closely connected with thewith the stanceestimation phase estimation result. Choosing Running_3 as an example, the phase stanceestimation phase estimation of stance phase result. Choosing Running_3 as an example, the stance of is only 4/37 leads whichtoleads totravelled a total travelled estimation of 686.4/100. The average SHOE isSHOE only 4/37 which a total distancedistance estimation of 686.4/100. The average total total length average is 100.4/100 comparing of mag-ZUPT. As an example movingmoving length average error of error SHOEofisSHOE 100.4/100 comparing 2.3/1002.3/100 of mag-ZUPT. As an example stanceduration phase duration calculated using Equation (36) and Figure 14 of is Mix_1 is shown in Figure 26. stance phase calculated using Equation (36) and Figure 14 of Mix_1 shown in Figure 26. It is obvious that peak-peak information can be to distinguish movement including It is obvious that peak-peak information can be used to used distinguish movement patternspatterns including and running. Stanceduration phase duration should be adjusted with different gait cycles. walkingwalking and running. Stance phase should be adjusted with different gait cycles.
24 of 30 24 of 30
run
run
run
run
still walk
walk
walk
walk still
Sampling points (a)
Stance phase duration
Magnetic field strength (Gs)
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695 walk
run
walk
walk
run
run
walk
run
Index of gait cycle (b)
Figure 26. Mid-stance Mid-stanceand andstance stancephase phaseduration duration Mix_1; is magnetic strength waveform Figure 26. ofof Mix_1; (a)(a) is magnetic fieldfield strength waveform and and mid-stances (peaks) extracted from this wave; (b) is stance phase duration with different gait mid-stances (peaks) extracted from this wave; (b) is stance phase duration with different gait cycles. cycles. Peak-peak information can represent patternswalking including running. Peak-peak information can represent movementmovement patterns including andwalking running.and Stance phase Stance phase duration is calculated according to peak-peak information. duration is calculated according to peak-peak information. Table travelled distance distance of of SHOE SHOE and and mag-ZUPT. mag-ZUPT. Table 5. 5. Total Total travelled Trajectory Trajectory Walking_1 Walking_1 Walking_2 Walking_2 Walking_3 Walking_3 Running_1 Running_1 Running_2 Running_2 Running_3 Running_3 Mix_1 Mix_1 Mix_2 Mix_3 Mix_2 Average error Mix_3 Average error
Actual Moving Length Actual Moving Length 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 100 None 100 None
Moving Length of SHOE Moving Length of SHOE 94.8 94.8 95.6 95.6 94.4 94.4 176.9 176.9 185.9 185.9 686.4 686.4 104.8 104.8 153.8 180.5 153.8 100.4/100 180.5 100.4/100
Moving Length of Mag-ZUPT Moving Length of Mag-ZUPT 98.5 98.5 98.6 98.6 97.2 97.2 96.6 96.6 98.2 98.2 104.7 104.7 102.1 102.1 98.1 101.5 98.1 2.3/100 101.5 2.3/100
6.3. HDIB Experiments 6.3. HDIB Experiments Six typical trajectories including walking on the level ground, ascending stairs and descending stairsSix aretypical takentrajectories to evaluateincluding HDIB performance. Description of the test trajectories aredescending shown in walking on the level ground, ascending stairs and Table 6. Height_1 and Height_2 are walking on the level ground indoor and outdoor, respectively. stairs are taken to evaluate HDIB performance. Description of the test trajectories are shown in Height_3–Height_6 areHeight_2 walking indoor. Table 6. Height_1 and are walking on the level ground indoor and outdoor, respectively. Height_3–Height_6 are walking indoor. Table 6. Description of the test trajectories.
Table 6. Description of the test trajectories. Trajectory Trajectory Height_1 Height_1 Height_2 Height_2 Height_3 Height_3 Height_4 Height_5 Height_4 Height_6 Height_5 Height_6
Height Change Description Height Change Description Walking on the level ground for 396 m, the height change is 0 m. Walking is 00 m. m. Walkingon onthe thelevel level ground ground for for 396 408 m, m, the the height height change change is Walking oneach the floor level is ground 408 m, the heightdescending change is 0 5m. Height of 3.55 m,for ascending 4 floors, floors, ascending 1 floor Height ascending 11 floor floor Heightof ofeach eachfloor floorisis3.55 3.55m, m, ascending ascending 44 floors, floors, descending 5 floors, ascending Heightof ofeach eachfloor floorisis3.55 3.55m, m, ascending descending4 1floors, floor and ascending 1 floorascending 1 floor Height descending 5 floors, Walking floor height from floorand 2 toascending 7 is 4.108 m, height from floor 7 to floor 11 is 3.792 m Height of from each floor 2isto 3.55 m,11, descending 1 floor 1 floor Walking from floor 2 to floor 11, height from floor 2 to 7 is 4.108 m, height from floor 7 to floor 11 is 3.792 m
Height estimation error root mean square (RMS) is utilized to evaluate the results of the six Height estimation error root mean square (RMS) is utilized to evaluate the results of the six trajectories, which is shown in Table 7. We can get the following conclusions from the results: trajectories, which is shown in Table 7. We can get the following conclusions from the results: 1
Barometer based height estimation method is heavily influenced by the environment. Outdoor ① Barometer based height estimation method is heavily influenced by the environment. Outdoor environment is more complex than indoor. RMS of Height-1 (indoor) is 1.18 m comparing with environment is more complex than indoor. RMS of Height-1 (indoor) is 1.18 m comparing with 0.19 m of Height_2 (outdoor). Generally speaking, height estimation error of barometer is from 0.19 m of Height_2 (outdoor). Generally speaking, height estimation error of barometer is from 1 m to 2 m which is about half of the floor height. 1 m to 2 m which is about half of the floor height. 2
MEMS-IMU based based height height estimation estimation method method has has aa large large error. error. However, However, the the short-time short-time accuracy accuracy ② MEMS-IMU of MEMS-IMU MEMS-IMU is is high high and and invulnerable invulnerable to to environment environment factors. factors. Height_5 Height_5 is is aa short short trajectory of trajectory with only one floor up and down and MEMS-IMU based method is even better than barometer. with only one floor up and down and MEMS-IMU based method is even better than barometer. 3
HDIB gets gets the the best best accuracy accuracy among among all all the the six six tests. tests. ③ HDIB
Sensors 2017, 17, 2695
25 of 30
Sensors 2017, 17, 2695
25 of 30
Table 7. RMS results of IMU based height estimation, barometer based height estimation and HDIB based height estimation. Table 7. RMS results of IMU based height estimation, barometer based height estimation and HDIB based height estimation. Total Height IMU Height Error Barometer Height HDIB Height Trajectory Change (m) RMS (m) BarometerError RMS (m) Error RMSError (m) Total Height Change IMU Height Error Height Error HDIB Height
Trajectory
Height_1
Height_1 Height_2 Height_2 Height_3 Height_3 Height_4 Height_4 Height_5 Height_5 Height_6 Height_6
(m) 0 0 35.5 35.5 7.1 35.7
0 0 35.5 35.5 7.1 35.7
RMS (m) 14.15 22.07 10.29 4.28 0.80 16.94
14.15 22.07 10.29 4.28 0.80 16.94
RMS (m) 0.19 0.19 1.18 1.18 1.31 1.31 1.78 1.78 0.96 0.96 1.07 1.07
RMS (m) 0 00 0 0.26 0.26 0.69 0.69 0.27 0.27 0.30 0.30
As an example, height estimation results of MEMS-IMU, barometer and HDIB of Height_4 MEMS-IMU, barometer barometer and and HDIB HDIB is is 4.28, 4.28, 1.78 1.78 and and 0.69 0.69 m, m, trajectory is shown in Figure 27. RMS of MEMS-IMU, respectively. It is respectively. is obvious obvious that that we we can can get get aa more more accurate accurate height height estimation estimationresult resultusing usingHDIB. HDIB.
Figure of of MEMS-IMU, Barometer andand HDIB of Height_4 trajectory. The Figure 27. 27. Height Heightestimation estimationresults results MEMS-IMU, Barometer HDIB of Height_4 trajectory. blue line, red line and black line are height estimation from MEMS-IMU, barometer and HDIB, The blue line, red line and black line are height estimation from MEMS-IMU, barometer and HDIB, respectively. The green green line line is is the the reference reference height height (true (true height). height). respectively. The
6.4. Comprehensive Pedestrian Trajectory Experiments 6.4. Comprehensive Pedestrian Trajectory Experiments Six comprehensive experiments are taken to evaluate the 3D positioning system. The key Six comprehensive experiments are taken to evaluate the 3D positioning system. The key indicators including height error and TTDE are summarized in Table 8. Traj_1 is ascending stairs indicators including height error and TTDE are summarized in Table 8. Traj_1 is ascending stairs from from floor 2 to floor 11, then walking around a rectangle in floor 11. Finally, the pedestrian returns to floor 2 to floor 11, then walking around a rectangle in floor 11. Finally, the pedestrian returns to the the starting point. The total travelled distance of Traj_1 is 261 m with 18 floors up and down. This starting point. The total travelled distance of Traj_1 is 261 m with 18 floors up and down. This scenario scenario is mainly to verify the height estimation promotion of HDIB. The 3D positioning result is is mainly to verify the height estimation promotion of HDIB. The 3D positioning result is shown in shown in Figure 28. TTDE of Traj_1 is 0.32% and the height error is 0.68 m. Figure 28. TTDE of Traj_1 is 0.32% and the height error is 0.68 m. Table 8. Key indicators of the six experiments. Table 8. Key indicators of the six experiments. Trajectory Total Moving Distance (m) Height Error (m) Trajectory (m) Height Error Traj_1 Total Moving Distance 261 0.68 (m) 0.27 Traj_1 Traj_2 261368 0.68 Traj_2 Traj_3 368409 0.270 Traj_3 Traj_4 409202 00 Traj_4 Traj_5 202 00.20 1430 Traj_5 Traj_6 1430 0.20 2696 2.35 Traj_6 2696 2.35
TTDE TTDE 0.32% 0.76% 0.32% 0.39% 0.76% 0.39% 0.77% 0.77% 0.63% 0.63% 1.04% 1.04%
Sensors 2017, 17, 2695 Sensors Sensors2017, 2017,17, 17,2695 2695
26 of 30 26 26ofof30 30
Figure 28. Positioning result the positioning result; (b) isisthe top view of Figure (a) isisthe the 3D view ofof the positioning result; (b) top view Figure28. 28.Positioning Positioningresult resultofofTraj_1; Traj_1;(a) (a)is the3D 3Dview viewof the positioning result; (b) isthe the top view the positioning result. TTDE of Traj_1 is 0.32% and the height error is 0.68 m. of the positioning result. TTDE of Traj_1 is 0.32% and the height error is 0.68 m. of the positioning result. TTDE of Traj_1 is 0.32% and the height error is 0.68 m.
Traj_2 Traj_2is isaaamix mixof ofwalking walkingon onthe theflat, flat,ascending ascendingand anddescending descendingthe thestairs. stairs.The Thetotal totaltravelled travelled Traj_2 is mix of walking on the flat, ascending and descending the stairs. The total travelled distance is 368 m with 5 floor up/down. The 3D and top view of Traj_2 positioning result is shown distance is 368 m with 5 floor up/down. The 3D and top view of Traj_2 positioning result is shown in distance is 368 m with 5 floor up/down. The 3D and top view result is shownin in Figure 29. TTDE of Traj_2 is 0.76% and the height error is 0.27 m. Figure 29. 29. TTDE TTDE of of Traj_2 Traj_2isis0.76% 0.76%and andthe theheight heighterror errorisis0.27 0.27m. m. Figure
Figure (a) isisthe the 3D view ofof the positioning result; (b) top view Figure 29. Positioning result the positioning result; (b) isisthe top view of Figure29. 29.Positioning Positioningresult resultofofTraj_2; Traj_2;(a) (a)is the3D 3Dview viewof the positioning result; (b) isthe the top view of the positioning result. TTDE of Traj_2 is 0.76% and the height error is 0.27 m. the positioning result. TTDE of Traj_2 is 0.76% and the height error is 0.27 m. of the positioning result. TTDE of Traj_2 is 0.76% and the height error is 0.27 m.
Traj_3 Traj_3is walking/runningmix mixtrajectory trajectoryalong alongaaarectangle rectangleof of555loops. loops.The Thepositioning positioningresult resultisis is Traj_3 isisaaawalking/running walking/running mix trajectory along rectangle of loops. The positioning result shown in Figure 30a and ZUPT results are shown in Figure 30b. The stance phase estimation of SHOE shown in Figure 30a and ZUPT results are shown in Figure 30b. The stance phase estimation of SHOE shown in Figure 30a and ZUPT results are shown in Figure 30b. The stance phase estimation of SHOE isis isless lessthan thanthe thereal realnumber numberwhich whichleads leadsto tothe thedivergence divergenceof ofthe thepositioning positioningresult. result.While Whilethe thestance stance less than the real number which leads to the divergence of the positioning result. While the stance phase estimation of mag-ZUPT performs well and the accumulative error is efficiently eliminated. phase estimation of mag-ZUPT performs well and the accumulative error is efficiently eliminated. phase estimation of mag-ZUPT performs well and the accumulative error is efficiently eliminated. TTDE TTDEof ofTraj_3 Traj_3is 0.39%. TTDE of Traj_3 isis0.39%. 0.39%. Traj_4 is a walking and running mix trajectory which isisdivided totofive walking segments Traj_4isisaawalking walkingand andrunning running mix trajectory which divided five walking segments and Traj_4 mix trajectory which is divided to five walking segments and and four four running segments. The positioning results and ZUPT comparison of SHOE and mag-ZUPT are four running segments. The positioning results and ZUPT comparison of SHOE and mag-ZUPT are running segments. The positioning results and ZUPT comparison of SHOE and mag-ZUPT are shown shown in Figure 31. Considering that SHOE is threshold based which is different in walking and shown in31. Figure 31. Considering thatisSHOE is threshold basediswhich is different in and walking and in Figure Considering that SHOE threshold based which different in walking running running gait patterns. In this experiment, SHOE-WALK and SHOE-RUNNING are adjusted to the running gait patterns. In this experiment, SHOE-WALK and SHOE-RUNNING are adjusted to the gait patterns. In this experiment, SHOE-WALK and SHOE-RUNNING are adjusted to the walking walking and running gait patterns, respectively. The threshold of SHOE-WALK is smaller than walking and running gait patterns, respectively. The threshold of SHOE-WALK is smaller than and running gait patterns, respectively. The threshold of SHOE-WALK is smaller than SHOE-RUN. SHOE-RUN. IfIfthe positioning system applies the calculated trajectory will be longer SHOE-RUN. thesystem positioning system appliesSHOE-WALK, SHOE-WALK, the calculated trajectory will bethe longer If the positioning applies SHOE-WALK, the calculated trajectory will be longer than real than the real because more stance phases are misjudged to swing phases. And if SHOE-WALK isis than the real because more stance phases are misjudged to swing phases. And if SHOE-WALK because more stance phases are misjudged to swing phases. And if SHOE-WALK is applied, the result applied, the result is contrary to the former one. More swing phases are misjudged to stance phases applied, the result is contrary to the former one. More swing phases are misjudged to stance phases is contrary to the former one. More swing phases are misjudged to stance phases and the calculated and andthe thecalculated calculatedtrajectory trajectoryisismuch muchshorter. shorter.However, However,mag-ZUPT mag-ZUPTisisnot notthreshold thresholdbased basedand andititwill will
Sensors 2017, 17, 2695
27 of 30
Sensors Sensors 2017, 2017, 17, 17, 2695 2695
27 27 of of 30 30
trajectory is much shorter. However, mag-ZUPT is not threshold based and it will self-adjust according self-adjust according the mid-stance and peak-peak Therefore, theisTTDE mag-ZUPT is to the mid-stance andtopeak-peak period. Therefore, theperiod. TTDE of mag-ZUPT 0.77%ofwhich is much 0.77% which is much better than SHOE. better than SHOE.
Figure 30. Positioning Positioning and ZUPT result of traditional ZUPT and mag-ZUPT of Traj_3; (a) (a) is Figure 30. Positioning and and ZUPT ZUPT result result of of traditional traditional ZUPT ZUPT and and mag-ZUPT mag-ZUPT of of Traj_3; is the the positioning positioning result. result. The The trajectory trajectory of of mag-ZUPT mag-ZUPT is is more matched to trajectory; (b) is ZUPT ZUPT The trajectory of mag-ZUPT is more matched matched to the the true true trajectory; (b) is information. information. Mag-ZUPT Mag-ZUPT based based stance stance phase estimation has better performance. performance. TTDE this test test Mag-ZUPT based stance phase estimation estimation has aa better TTDE of of this is 0.39%. is 0.39%.
Figure the positioning Figure 31. 31. Positioning Positioning results results and and ZUPT ZUPT comparison comparison of of Traj_4; Traj_4; (a) (a) is is the the positioning positioning result. result. Blue Blue line line and red line are the results of SHOE-ZUPT method whose threshold is adjusted to walking and and red redline lineare are the results of SHOE-ZUPT method whose threshold is adjusted to walking and the results of SHOE-ZUPT method whose threshold is adjusted to walking and running, running, is result of novel method in running, respectively. respectively. Black line is the the result of the the novel mag-ZUPT mag-ZUPT method developed developed in this this paper; respectively. Black lineBlack is theline result of the novel mag-ZUPT method developed in this paper; (b)paper; is the (b) is the ZUPT result of SHOE-WALK, SHOE-RUN and mag-ZUPT. TTDE of mag-ZUPT is (b) is the ZUPT result of SHOE-WALK, SHOE-RUN and mag-ZUPT. TTDE of mag-ZUPT 0.77%. ZUPT result of SHOE-WALK, SHOE-RUN and mag-ZUPT. TTDE of mag-ZUPT is 0.77%. is 0.77%.
Traj_5 and Traj_6 are long distance indoor-outdoor tests. The start point is on floor 6. The Traj_5 and Traj_6 are long distance indoor-outdoor tests. The start point is on floor 6. pedestrian walks down to the outdoor and walks in the campus, then goes up 6 floors to the start The pedestrian walks down to the outdoor and walks in the campus, then goes up 6 floors to the point. Traj_6 with one loop along the stadium is more complex than Traj_5. Positioning results of start point. Traj_6 with one loop along the stadium is more complex than Traj_5. Positioning results Traj_5 and Traj_6 are shown in Figures 32 and 33, respectively. TTDE of Traj_5 is 0.63% and the height of Traj_5 and Traj_6 are shown in Figures 32 and 33, respectively. TTDE of Traj_5 is 0.63% and the error is 0.20 m. TTDE of Traj_6 is 1.04% and the height error is 2.35 m.The results show that the height error is 0.20 m. TTDE of Traj_6 is 1.04% and the height error is 2.35 m.The results show method described in this paper can obtain a good 3D positioning result of the complex that the method described in this paper can obtain a good 3D positioning result of the complex indoor/outdoor trajectory. indoor/outdoor trajectory.
Sensors 2017, 17, 2695 Sensors 2017, 17, 2695 Sensors 2017, 17, 2695
28 of 30 28 of 30 28 of 30
Figure 32. Positioning result of Traj_5 overlaid on a satellite campus picture; (a) is the top view; (b) is Figure 32. Positioning result of overlaid on satellite campus picture; (a) is Figure Positioning result of Traj_5 Traj_5 on aaand satellite campus picture; (a)m. is the the top top view; view; (b) (b) is is height 32. estimation result. TTDE of this overlaid test is 0.63% the height error is 0.20 height estimation result. TTDE of this test is 0.63% and the height error is 0.20 m. height estimation result. TTDE of this test is 0.63% and the height error is 0.20 m.
Figure33. 33.Positioning Positioningresult resultof ofTraj_6 Traj_6overlaid overlaidon onaasatellite satellitecampus campuspicture; picture;(a) (a)isisthe thetop topview; view;(b) (b)isis Figure Figure 33. Positioning result of Traj_6 overlaid on a satellite campus picture; (a) is the top view; (b) is heightestimation estimationresult. result.TTDE TTDEof ofthis thistest testisis1.04% 1.04%and andthe theheight heighterror errorisis2.35 2.35m. m. height height estimation result. TTDE of this test is 1.04% and the height error is 2.35 m.
Conclusions 7.7. Conclusions 7. Conclusions Anovel novel3D 3Dpedestrian pedestriannavigation navigationmethod methodintegrating integratingaaMEMS-IMU, MEMS-IMU,barometer barometerand andpermanent permanent A A novel 3D pedestrian navigation method integrating a MEMS-IMU, barometer and permanent magnet is presented in this paper to solve the stance phase detection, height estimation and total total magnet is presented in this paper to solve the stance phase detection, height estimation and magnet is distance presented in this paper to solve the stance phase detection, height estimation and total travelled calculation problems of foot-mounted inertial systems. Stance phase could be travelled distance calculation problems of foot-mounted inertial systems. Stance phase could be travelled distance calculation problems of foot-mounted inertial systems. Stance phase could be misjudgedusing usingacceleration accelerationand andangular angularrate-based rate-basedZUPT ZUPTdetector detectorlike likeSHOE, SHOE,especially especiallyin inrunning. running. misjudged misjudged using acceleration and angular rate-based ZUPTofdetector like SHOE, permanent especially inmagnet running. Mag-ZUPT applying magnetic field strength information the foot-mounted Mag-ZUPT applying magnetic field strength information of the foot-mounted permanent magnet isis Mag-ZUPT magnetic fieldStance strength information the foot-mounted permanent is proposedto toapplying estimatestance stancephase. phase. phase durationof calculated usingthe the proposedmagnet modelof of proposed estimate Stance phase duration isiscalculated using proposed model proposed to estimate stance phase. Stance phase duration is calculated using the proposed model of stance phase duration and peak-peak information. With stance phase duration, mid-stance stance phase duration and peak-peak information. With stance phase duration, mid-stance is expandedis stance phase duration and peak-peak information. Withofstance phase duration, mid-stance is expanded to stance HDIB theofadvantages MEMS-IMU barometer provide to stance phase. HDIBphase. combines thecombines advantages MEMS-IMU and barometerand to provide hightoaccuracy expanded to stance phase. HDIB combines the advantages MEMS-IMU and barometer high accuracy height This has of two stages which are levelto provide ground/ height estimation. This estimation. algorithm has twoalgorithm stages which are level ground/ upstairs/downstairs high accuracy height estimation. This algorithm has two stages which are level ground/ upstairs/downstairs distinction and height calculation. ZUPT-AAWL is proposed using stance distinction and height calculation. ZUPT-AAWL is proposed using stance phase informationphase in a upstairs/downstairs distinction and calculation. ZUPT-AAWL is proposed stance phase information in athe gaitaverage cycle towindow set theheight average window length to calculate thedistance totalusing travelled distance gait cycle to set length to calculate the total travelled more precisely. information in a gait to set the average window length to the calculate the total travelled more precisely. The cycle biggest difference ZUPT-AAWL and algorithm is distance that the The biggest difference of ZUPT-AAWL andofthe traditional algorithm istraditional that the average window length more precisely. The biggest difference of ZUPT-AAWL and the traditional algorithm is that the average window length is adaptively changed according to different gait cycles. is adaptively changed according to different gait cycles. average window length is adaptively changed according to different cycles. Three kindsof ofexperiments experiments aretaken taken toevaluate evaluate performance ofgait theproposed proposed method.The Thefirst first Three kinds are to performance of the method. Three kinds of experiments are taken to evaluate performance of the proposed method. Thephase first one includes nine tests of walking/running/mix trajectories. It aims to compare the stance one includes nine tests of walking/running/mix trajectories. It aims to compare the stance phase one includes nine tests of ofSHOE walking/running/mix It aimsthat to mag-ZUPT compare theis stance phase detection performance and mag-ZUPT. mag-ZUPT. trajectories. The results results show show much better, better, detection performance of SHOE and The that mag-ZUPT is much detection SHOE andone mag-ZUPT. The results thatwalking mag-ZUPT muchground, better, especiallyperformance in running. running. of The tests which show include on aaislevel level especially in The second second one has has six six tests which include walking on ground, especially in running. The second one has six tests which include walking on a level ground, ascending stairs and descending stairs to evaluate HDIB performance. The results show that RMS of ascending stairs and descending stairs to evaluate HDIB performance. The results show that RMS of
Sensors 2017, 17, 2695
29 of 30
ascending stairs and descending stairs to evaluate HDIB performance. The results show that RMS of height estimation of the six tests are all less than one meter, which is much better than MEMS-IMU and barometer alone. The last one contains six comprehensive trajectories including indoor/outdoor, walking/running, ascending/descending stairs. Numerical results show that TTDE of these experiments ranges from 0.32% to 1.04% in both walking and running gait patterns, and the height estimation error is from 0 m to 2.35 m. This 3D positioning method can obtain a good height and TTDE accuracy in both walking and running gait patterns. In our future work, we will use low-cost MEMS-IMU to achieve a similar TTDE performance. More experiments will be taken to establish a more accurate model of stance phase duration and peak-peak period information. Acknowledgments: This study is supported by the Beihang Beidou Technology Industrialization Funding program (Grant No.: BARI1701). Author Contributions: Wei Yang, Chundi Xiu, Jianmin Zhang and Dongkai Yang conceived and designed the experiments; Wei Yang and Jianmin Zhang performed the experiments; Wei Yang wrote the paper. All of the work in this paper is done under the supervision of Chundi Xiu and Dongkai Yang. Conflicts of Interest: The authors declare no conflict of interest.
References 1. 2. 3. 4.
5. 6. 7.
8. 9. 10.
11.
12. 13. 14. 15.
Kaplan, E.D.; Hegarty, C. Understanding GPS: Principles and Applications; Artech House: Norwood, MA, USA, 2005; pp. 598–599. Mautz, R. Indoor Positioning Technologies. Ph.D. Thesis, ETH Zurich, Zurich, Switzerland, 2012. Du, Y.; Yang, D.; Xiu, C. A Novel Method for Constructing a WIFI Positioning System with Efficient Manpower. Sensors 2015, 15, 8358–8381. [CrossRef] [PubMed] Youssef, M.A.; Agrawala, A.; Shankar, A.U. WLAN Location Determination via Clustering and Probability Distributions. In Proceedings of the IEEE International Conference on Pervasive Computing and Communications, Fort Worth, TX, USA, 23–26 March 2003; pp. 143–150. Du, Y.; Yang, D.; Yang, H.; Xiu, C. Flexible indoor localization and tracking system based on mobile phone. J. Netw. Comput. Appl. 2016, 69, 107–116. Li, X.; Wang, J.; Liu, C. A Bluetooth/PDR Integration Algorithm for an Indoor Positioning System. Sensors 2015, 15, 24862–24885. [CrossRef] [PubMed] Diaz, J.J.M.; Maues, R.D.A.; Soares, R.B.; Nakamura, E.F.; Figueiredo, C.M.S. Bluepass: An indoor Bluetooth-based localization system for mobile applications. In Proceedings of the IEEE Symposium on Computers and Communications, Riccione, Italy, 22–25 June 2010; pp. 778–783. Shirehjini, A.A.; Yassine, A.; Shirmohammadi, S. Equipment location in hospitals using RFID-based positioning system. IEEE Trans. Inf. Technol. Biomed. 2012, 16, 1058–1069. [CrossRef] [PubMed] Jourdan, D.; Dardari, D.; Win, M.Z. Position error bound for UWB localization in dense cluttered environments. IEEE Trans. Aerosp. Electron. Syst. 2008, 44, 613–628. [CrossRef] Zhang, C.; Kuhn, M.J.; Merkl, B.C.; Fathy, A.E. Real-Time Noncoherent UWB Positioning Radar with Millimeter Range Accuracy: Theory and Experiment. IEEE Trans. Microw. Theory Tech. 2010, 58, 9–20. [CrossRef] Yoshino, M.; Haruyama, S.; Nakagawa, M. High-accuracy positioning system using visible LED lights and image sensor. In Proceedings of the Radio and Wireless Symposium, Orlando, FL, USA, 22–24 January 2008; pp. 439–442. Holm, S. Hybrid ultrasound-RFID indoor positioning: Combining the best of both worlds. In Proceedings of the IEEE International Conference on RFID, Orlando, FL, USA, 27–28 April 2009; pp. 155–162. Zhang, R.; Hoflinger, F.; Reindl, L. Inertial Sensor Based Indoor Localization and Monitoring System for Emergency Responders. IEEE Sens. J. 2013, 13, 838–848. [CrossRef] Zheng, L.; Zhou, W.; Tang, W.; Zheng, X.; Peng, A.; Zheng, H. A 3D indoor positioning system based on low-cost MEMS sensors. Simul. Model. Pract. Theory 2016, 65, 45–56. [CrossRef] Foxlin, E. Pedestrian Tracking with Shoe-Mounted Inertial Sensors. IEEE Comput. Graph. Appl. 2005, 25, 38–46. [CrossRef] [PubMed]
Sensors 2017, 17, 2695
16.
17.
18.
19. 20. 21. 22.
23. 24. 25.
26. 27. 28. 29. 30.
31.
32.
33.
34.
30 of 30
Jiménez, A.R.; Seco, F.; Prieto, J.C.; Guevara, J. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU. In Proceedings of the 2010 7th Workshop on Positioning Navigation and Communication, Dresden, Germany, 11–12 March 2010; pp. 135–143. Nilsson, J.O.; Gupta, A.K.; Handel, P. Foot-mounted inertial navigation made easy. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation, Busan, Korea, 27–30 October 2014; pp. 24–29. Nilsson, J.O.; Skog, I.; Handel, P.; Hari, K.V.S. Foot-mounted INS for everybody—An open-source embedded implementation. In Proceedings of the Position Location and Navigation Symposium, Myrtle Beach, SC, USA, 23–26 April 2012; pp. 140–145. Fischer, C.; Sukumar, P.T.; Hazas, M. Tutorial: Implementing a Pedestrian Tracker Using Inertial Sensors. IEEE Pervas. Comput. 2013, 12, 17–27. [CrossRef] Wang, Z.; Zhao, H.; Qiu, S.; Gao, Q. Stance-Phase Detection for ZUPT-Aided Foot-Mounted Pedestrian Navigation System. IEEE/ASME Trans. Mechatron. 2015, 20, 3170–3181. [CrossRef] Skog, I.; Handel, P.; Nilsson, J.O.; Rantakokko, J. Zero-Velocity Detection—An Algorithm Evaluation. IEEE Trans. Biomed. Eng. 2010, 57, 2657–2666. [CrossRef] [PubMed] Skog, I.; Nilsson, J.O.; Händel, P. Evaluation of zero-velocity detectors for foot-mounted inertial navigation systems. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation, Zurich, Switzerland, 15–17 September 2010; pp. 1–6. Zhou, C.; Downey, J.; Stancil, D.; Mukherjee, T. A Low-Power Shoe-Embedded Radar for Aiding Pedestrian Inertial Navigation. IEEE Trans. Microw. Theory Tech. 2010, 58, 2521–2528. [CrossRef] Zhou, C.; Downey, J.; Choi, J.; Stancil, D. A Shoe-Embedded RF Sensor for Motion Detection. IEEE Microw. Wirel. Compon. Lett. 2011, 21, 169–171. [CrossRef] Bebek, O.; Suster, M.A.; Rajgopal, S.; Fu, M.J. Personal navigation via shoe mounted inertial measurement units. In Proceedings of the 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Taipei, Taiwan, 18–22 October 2010; Volume 25, pp. 1052–1058. Norrdine, A.; Kasmi, Z.; Blankenbach, J. Step Detection for ZUPT-Aided Inertial Pedestrian Navigation System Using Foot-Mounted Permanent Magnet. IEEE Sens. J. 2016, 16, 1. [CrossRef] Wang, J.S.; Lin, C.W.; Yang, Y.T.; Ho, Y.J. Walking pattern classification and walking distance estimation algorithms using gait phase information. IEEE Trans. Biomed. Eng. 2012, 59, 2884–2892. [CrossRef] [PubMed] Godha, S.; Lachapelle, G. Foot mounted inertial system for pedestrian navigation. Meas. Sci. Technol. 2008, 19, 75202. [CrossRef] Lee, J.A.; Cho, S.H.; Lee, Y.J.; Yang, H.K.; Lee, J.W. Portable activity monitoring system for temporal parameters of gait cycles. J. Med. Syst. 2010, 34, 959–966. [CrossRef] [PubMed] Xia, H.; Wang, X.; Qiao, Y.; Jian, J.; Chang, Y. Using Multiple Barometers to Detect the Floor Location of Smart Phones with Built-in Barometric Sensors for Indoor Positioning. Sensors 2015, 15, 7857–7877. [CrossRef] [PubMed] Li, B.; Harvey, B.; Gallagher, T. Using barometers to determine the height for indoor positioning. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation, Montbeliard-Belfort, France, 28–31 October 2013; pp. 1–7. Romanovas, M.; Goridko, V.; Klingbeil, L.; Bourouah, M.; Al-Jawad, A.; Traechtler, M.; Manoli, Y. Pedestrian Indoor Localization Using Foot Mounted Inertial Sensors in Combination with a Magnetometer, a Barometer and RFID; Springer: Berlin/Heidelberg, Germany, 2013; pp. 151–172. Ruiz, A.R.J.; Granja, F.S.; Prieto Honorato, J.C.; Rosas, J.I.G. Accurate Pedestrian Indoor Navigation by Tightly Coupling Foot-Mounted IMU and RFID Measurements. IEEE Trans. Instrum. Meas. 2012, 61, 178–189. [CrossRef] Ilyas, M.; Cho, K.; Baeg, S.H.; Park, S. Drift Reduction in Pedestrian Navigation System by Exploiting Motion Constraints and Magnetic Field. Sensors 2016, 16, 1455. [CrossRef] [PubMed] © 2017 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).
