A Novel Pedestrian Navigation Algorithm for a Foot-Mounted Inertial-Sensor-Based System

Abstract

This paper proposes a novel zero velocity update (ZUPT) method for a foot-mounted pedestrian navigation system (PNS). First, the error model of the PNS is developed and a Kalman filter is built based on the error model. Second, a novel zero velocity detection algorithm based on the variations in speed over a gait cycle is proposed. A finite state machine including three states is employed to model a gait cycle. The state transition conditions are determined based on speed using a sliding window. Third, the ZUPT software flow is illustrated and described. Finally, the performances of the proposed method and other methods are examined and compared experimentally. The experimental results show that the mean relative accuracy of the proposed method is 0.89% under various motion modes.

1. Introduction

The rapid development of micro-electromechanical systems (MEMS) has facilitated the production of inexpensive, lightweight and small-sized inertial sensors with low power consumption. These properties are desirable for a PNS. PNS’ that employ MEMS inertial sensors are proposed in [1,2,3,4,5,6]. In this paper, a strapdown inertial navigation algorithm [7,8] is applied to a PNS. Such a PNS is useful for locating and guiding emergency first responders, blind individuals and security personnel.

The major challenge of MEMS-based PNS concerns how to restrain the rapid accumulation of navigation errors. For example, position error is proportional to time cubed. Without an error-resetting algorithm, this error can exceed a meter in ten seconds [1]. A zero velocity update (ZUPT) algorithm could be employed if the PNS is mounted on a shoe. During normal walking cycles, a foot periodically returns to a stationary state and remains on the ground for a brief period of time (approximately 0.1~0.3 s); this interval is referred to as the zero velocity interval. When a stationary state is detected, the velocity error can be employed as an observation to estimate and can correct the sensor bias errors, attitude errors and position errors using a Kalman filter [9,10]. Researches show that the performance of a PNS can be significantly increased using a ZUPT algorithm.

The detection of the above static periods is a critical step in ZUPT, and many detection methods have been developed [5,9,10,11]. In [11], the zero velocity intervals are determined based on a likelihood ratio test (LRT) detector. The detector provides good performance at low gait speeds (approximately 0.83 m/s). In [5], an algorithm based on a hidden Markov model is constructed using a segmentation of gyroscope outputs. The algorithm shows good reliability under walking and running conditions. However, the state transition model is complex and not easy to implement. In [9], a stance phase detector that consists of one footstep detector and two zero velocity detectors is proposed. The detector can successfully detect zero velocity during walking, stair climbing, and running. However, the detector is easily confused when the pedestrian randomly alternates between walking and running. In this paper, a zero velocity detection algorithm based on the variations in speed during a gait cycle is proposed.

This paper is organized into seven sections. In Section 2, the inertial navigation algorithm of the PNS is provided. In Section 3, the dynamic and measurement models for the ZUPT Kalman filter are summarized. In Section 4, foot-stance phase detection based on variations in speed is presented. In Section 5, the software flow of the PNS is provided. The performance of the proposed method is experimentally verified in Section 6. The last section includes the conclusion and directions for future studies.

2. Inertial Navigation Algorithm of the PNS

The inertial navigation algorithm of a PNS is similar to traditional inertial navigation algorithm [12,13]; however, certain simplifications are made based on the characteristics of a PNS.

The body frame is defined as a right-handed (x,y,z) Cartesian coordinate system (x—forward, y—left, z—upward). The navigation frame is defined as east-north-up (ENU). We use the subscripts b (body) and n (navigation) to denote the project of a vector in a corresponding frame.

The traditional complete form of the velocity differential is:

$${\dot{V}}^n=C_b^n{f}_{ib}^b-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times V^n+g^n$$

(1)

where $C_b^n$ is the transformation matrix from the b frame to the n frame and $f_{ib}^b$ is the specific force in the body frame.

For a PNS, the term $(2{\omega }_{ie}^n+{\omega }_{en}^n)\times V^n$ can be omitted, and $g^n$ can be regarded as approximately constant.

The differential equation of the attitude is:

$${\dot{C}}_b^n=C_b^n({\omega }_{nb}^b\times )$$

(2)

${\omega }_{nb}^b$ represents the angular rate of the body frame relative to the navigation frame. ${\omega }_{nb}^b\times$ represents the cross product matrix of ${\omega }_{nb}^b$.

The position can be updated using the following difference equation:

$$R^n(k)=R^n(k-1)+V^n(k)T_s$$

(3)

where T_s is the sampling period and is set to 10 m·s. The calculation period of the attitude, velocity and position is equal to T_s.

3. ZUPT Kalman Filter

3.1. Error Model of Attitude

Let $\Psi$ be the rotation vector from the navigation frame $n$ to the computed navigation frame $n^{\prime }$; then:

$$\dot{\Psi }=-{\epsilon }^n=-C_b^n{\epsilon }^b=-C_b^n({\epsilon }_r^b+{\epsilon }_g^b)$$

(4)

where $\epsilon$ is the vector of the gyroscope measurement error, ${\epsilon }_r$ is white noise, and ${\epsilon }_g$ is the gyro bias. The gyro bias is regarded as a first-order Markov process, ${\dot{\epsilon }}_g=-\frac{1}{T_g}{\epsilon }_g+{\omega }_g$, where T_g is the correlation time, and ω_g is the driven noise with covariance ${\sigma }_g^2$.

3.2. Error Model of Velocity

The error model of velocity is given by:

$$\delta {\dot{V}}^n=f^n\times \psi +{\nabla }^n=C_b^n{f}^b\times \psi +C_b^n{\nabla }^b$$

(5)

where ${\nabla }^b={\nabla }_r^b+{\nabla }_a^b$ is the vector of the accelerometer measurement error, ${\nabla }_r$ is white noise, and ${\nabla }_a$ is the accelerometer bias. The accelerometer bias is modeled as a first-order Markov process, namely, ${\dot{\nabla }}_a=-\frac{1}{T_a}{\nabla }_a+{\omega }_a$ where $T_a$ is the correlation time and ${\omega }_a$ is the driven noise with covariance ${\sigma }_a^2$.

3.3. Error Model of Position

The error model of position is expressed as:

$$\delta {\dot{R}}^n=\delta V^n$$

(6)

3.4. Kalman Filter Equations

The state vector that is employed in the KF has the form: $x=({\Psi }^b,\delta v^n,{\epsilon }_g^b,{\nabla }_a^b)$ where ${\Psi }^b$ represents attitude errors, $\delta v^n$ is the velocity errors, ${\epsilon }_g^b$ is the gyro bias., and ${\nabla }_a^b$ is the accelerometer bias.

The dynamic error model of the Kalman filter is given by Equation (7):

$$\dot{x}=\left[\begin{array}{cccc}0& 0& -c_b^{n}I& 0\\ f^n\times & 0& 0& c_b^n\\ 0& 0& -\frac{1}{T_g}I& 0\\ 0& 0& 0& -\frac{1}{T_a}I\end{array}\right]x+\left[\begin{array}{c}{\epsilon }_r\\ {\nabla }_r\\ {\omega }_g\\ {\omega }_a\end{array}\right]$$

(7)

The measurement equation can be modeled as:

$$Z=Hx+v$$

(8)

$$H=[{\begin{array}{ccc}{0}_{3\times 3}& I_{3\times 3}& 0\end{array}}_{6\times 3}]$$

(9)

The observation $\delta v$ is the difference between the INS velocity and zero, $v$ is the measurement noise and $E[v(t_1)v(t_2)]=Q_v\delta (t_1-t_2)$. When the ZUPT is applied, the pedestrian’s foot is usually not perfectly stationary. The uncertainty is modeled in the measurement covariance matrix. Q_v is set to (0.05 m/s)²I_3×3 based on experimental results.

3.5. Feedback Compensation

The estimated state values are used as feedback to the navigation algorithm to correct the corresponding errors. Observation analysis indicates that the rank of the observation matrix $T_{obser}$ is nine, which is less than twelve. Additional analyses conclude that heading error and gyroscope bias errors cannot be observed. Thus, heading error and ${\epsilon }_g^b$ are not used as feedback for the filter.

The position errors are corrected using Equation (10):

$$\delta R(t)=\frac{1}{2}\delta V^n(t)t$$

(10)

4. Zero Velocity Detection

4.1. Variations in Velocity in a Gait Cycle

Most zero velocity detection methods employ comparisons between thresholds and the magnitude of acceleration, magnitude of angular rate, or their combinations. The primary constraint of these methods is that the variations in acceleration and angular rate differ greatly under various motion modes, such as walking, running, and stair climbing. Thus, it is difficult to find a threshold function or threshold value that is widely applicable. We demonstrate this in Figure 1 and Figure 2.

Figure 1. The detected zero velocity interval when a pedestrian is walking. The red line represents the stationary state and moving state. Small values indicate the stationary state. Large values indicate the moving state.

Figure 1. The detected zero velocity interval when a pedestrian is walking. The red line represents the stationary state and moving state. Small values indicate the stationary state. Large values indicate the moving state.

Figure 2. The detected zero velocity interval when a pedestrian is running. In these time intervals, there are no false detections for the accelerometer (upper). However, only two zero velocity intervals are detected for the gyroscope (lower).

Figure 2. The detected zero velocity interval when a pedestrian is running. In these time intervals, there are no false detections for the accelerometer (upper). However, only two zero velocity intervals are detected for the gyroscope (lower).

Two traditional methods are employed: comparisons of acceleration magnitudes with a threshold and comparisons of angular rate magnitudes with a threshold. Figure 1 shows that although the angular rate magnitude detector performs better than the acceleration-magnitude detector, both methods produce false positives during walking. Figure 2 shows that the acceleration magnitude detector performs better than the angular rate magnitude detector during running; the latter failed to detect most of the zero velocity intervals.

As shown in Equations (1) and (2), the calculation of the velocity uses both acceleration and angular rate data; thus, we attempt to use speed (the norm of velocity) to detect zero velocity intervals. Figure 3 shows the speeds that correspond to walking, running, stair climbing and stair descending. The variations in speed during a gait cycle are similar under different motion modes, except magnitude, duration and certain local details. A comparison of Figure 3 with Figure 1 and Figure 2 show that the variation in speed is simpler and more distinct than the variations in acceleration and angular rate.

Figure 3. Norm of velocity for different types of gait.

Figure 3. Norm of velocity for different types of gait.

Figure 4. Speed trend and human limb kinematic in a typical walking cycle.

Figure 4. Speed trend and human limb kinematic in a typical walking cycle.

Figure 4 shows the speed trend and a human limb kinematic during a typical walking cycle. The lower panel is taken from [14]. During a walking cycle, the speed changes from acceleration to deceleration, and then changes back to zero. The acceleration interval corresponds to the change from the heel lift to the swing at the highest point. The deceleration interval corresponds to the change from the highest point to a flat foot. The zero velocity intervals correspond to the change from a flat foot to a heel lift.

4.2. Hidden Markov Model for Zero Velocity Detection

We introduce a hidden Markov model for zero velocity detection. Gait cycles are modeled as a finite state machine and each gait cycle is divided into three states. The state transition diagram is shown in Figure 5. State 1 corresponds to zero velocity intervals. State 2 corresponds to acceleration intervals. State 3 corresponds to deceleration intervals.

Figure 5. State transition diagram.

Figure 5. State transition diagram.

A sliding window is used to determine state transitions. The size of the sliding window is $N\times T_s$. The speeds of the $N+1$ moments, $t_k,t_{k+1}\cdots t_{k+N+1}$, are saved in a buffer. If the current state is state 1 and the slope of the speed in the window, denoted by $slope\_s$, is larger than the threshold Th_a (condition 1), then the state moves to state 2. If the current state is state 2 and the speed of $t_k$ is the maximum speed in the window (condition 2), the state moves to state 3. If the current state is state 3 and the speed of t_k is the minimum speed in the window (condition 3), the state moves to state 1. Th_a is set to 0.3 m/s². N is set to 20.

The first order polynomial fitting method is employed to calculate the slope of the speed, as shown in Equation (11):

$$w\_s(i)=s_0+s_1\times (i-1)\times T+z_i,i=1,2,\cdots N+1$$

(11)

slope_s=s₁ is the slope of the speed, which can be interpreted as the mean acceleration in the window.

5. PNS Software Flow

The flow chart for the PNS software algorithm is shown in Figure 6.

During the initial alignment, the PNS is initialized with the attitude and position. The initial pitch and roll are calculated using accelerometer data [15], The initial azimuth is calculated using magnetometer data [15], the outdoor position is initialized using GPS data, and the indoor position is initialized using the building plan data, as shown in [2].

Figure 6. Pedestrian navigation system software flow chart.

Figure 6. Pedestrian navigation system software flow chart.

The bias of the gyroscope is estimated during initial alignment as well, too. The average of the gyroscope data is calculated and used as the approximate gyroscope bias. If the variance of the gyroscope data is greater than a preset threshold, or if the zero velocity detector has detected motion of pedestrian in less than 6 s, the estimation of the gyroscope bias is skipped.

The matrices G_3×N and A_3×N are employed to store the sampling data of the gyroscopes and accelerometers in the window. The matrices C_3×3(N+1), V_3×(N+1), P_3×(N+1) and S_1×(N+1) are employed to store the attitude matrix, velocity, position and speed in the window.

The zero velocity detection algorithm decides whether the velocity at the starting moment of the window is zero. If the zero velocity moment is detected, attitude errors, velocity errors, and position errors are estimated by the Kalman filter and compensated; then, the sampling data in G_3×N and A_3×N are used to recalculate the attitude, velocity and position in the window. After each navigation calculation and ZUPT calculation, C_3×3(N+1) is left shifted by three columns, and G_3×N, A_3×N, V_3×(N+1), P_3×(N+1) and S_1×(N+1) are left shifted by one column.

6. Experiments

6.1. The Inertial Measurement Unit

The inertial measurement unit that we employed is the model MTI-G from Xsens Technologies B.V. (Enschede, The Nethelands). It is configured to provide inertial data at 100 Hz. The IMU includes three MEMS accelerometers, three MEMS gyroscopes and three magnetometers. The gyroscope bias repeatability is 0.5 deg/s (max), and the in-run bias stability is 10 deg/h (typical). The accelerometer bias repeatability is 0.05 m/s² (max),and the in-run bias stability is 40 μg (typical). The IMU is strapped on to the front of a shoe or the rear part side of a shoe, as shown in Figure 7.

Figure 7. (Left) IMU strapped on the front of a shoe; (Right) IMU strapped on the rear side of a shoe.

Figure 7. (Left) IMU strapped on the front of a shoe; (Right) IMU strapped on the rear side of a shoe.

6.2. Zero Velocity Detection

The experiments were conducted using a pedestrian, a 25-year-old male with a height of 1.73 m and a weight of 70 kg. The IMU was strapped on to the front of a shoe. The pedestrian conducted six types of motion: walking, running, stair climbing, stair descending, uphill and downhill. The average speeds are approximately 1.35, 2.2, 0.7, 1.09, 0.8 and 1.4 m/s, respectively. Figure 8 shows the speeds and the corresponding zero velocity detector states. The blue line represents the speeds, and the red line represents the state. “0” denotes state 1, “1” denotes state 2 and “2” denotes state 3. Figure 8 shows that the zero velocity detector state well matches the variation in speed under all motion modes.

Figure 8. Zero velocity detection using the method proposed in this paper.

Figure 8. Zero velocity detection using the method proposed in this paper.

To provide a comparison, we examine the performance of the algorithm in [11] using the same experimental data. The detected zero velocity intervals are shown in Figure 9. As illustrated by panel (a), except for one interval, none of the zero velocity intervals, are detected during running. As illustrated by panel (b), none of the zero velocity intervals are detected during stair climbing. Moreover, the detected zero velocity intervals during stair descending are incorrect. As illustrated by panel (c), some of the detected zero velocity intervals during uphill and downhill are correct, although others are incorrect.

Figure 9. Zero velocity detection using method proposed in [11]. The vertical unite is dimensionless numbers.

Figure 9. Zero velocity detection using method proposed in [11]. The vertical unite is dimensionless numbers.

To examine whether the mounting position of the IMU affects the effectiveness of the proposed algorithm, we conducted experiments with the IMU strapped onto the rear side of a shoe, as illustrated in Figure 7. Six types of motion were performed by a pedestrian, specifically, a 30-year-old male with a height of 1.70 m and a weight of 73 kg. The speed and zero velocity detector states are illustrated in Figure 10, from which it can be observed that the proposed algorithm performed well.

To test the robustness of the algorithm, we randomly selected three other people from the Beijing University of Technology to conduct the experiments. They wore different types of shoes, including leather shoes and sport shoes. One pedestrian conducted the experiments in the same location used by the 25-year-old male. Another two pedestrians conducted the experiment in randomly selected locations in our university. We asked the pedestrians to conduct six motion modes as before. The IMU was strapped onto the front of a shoe. The users can be described as follows: a 26-year-old female with a height of 1.58 m and a weight of 49 kg, a 40-year-old male with a height of 1.78 m and a weight of 62 kg, and a 38-year-old female with a height of 1.60 m and a weight of 51 kg. We carefully compare the performance of the proposed algorithm and the algorithm of [11] and draw the following conclusion:

(1)

The algorithm presented in [11] performs well during slow walking but does not perform well during running, stair climbing and descending, uphill and downhill.

(2)

The algorithm proposed in this paper performs well under all the above-mentioned types of motion.

Figure 10. Zero velocity detection of six types of gait when the IMU is strapped onto the rear side of a shoe. The parameters are the same as those in Figure 8.

Figure 10. Zero velocity detection of six types of gait when the IMU is strapped onto the rear side of a shoe. The parameters are the same as those in Figure 8.

6.3. Field Test Experiments

Experiments under three motion modes, including walking, running and alternating between walking and running, were conducted around the boundary line of the sports field at the Beijing University of Technology. The travelled distance is approximately 422 m. The calculated trajectories of the proposed algorithm and of that in [11] are shown in Figure 11. The IMU was strapped onto the front of a shoe. The blue line (walking alternating with running) and light blue (walking) line are the calculated trajectories of algorithm [11]. The red line (walking), yellow line (walking alternating with running) and green line (running) are calculated trajectories of the proposed method. The calculated trajectory of method [11] under the running mode is not illustrated because of the excessive position errors. The mean error (start/end distance) of the proposed algorithm is approximately 4.64 m, or approximately 1.10% of the travelled distance. The mean error (start/end distance) of the algorithm [11] is approximately 15.82 m, or approximately 3.75% of the travelled distance.

We also conducted experiments along another more complex trajectory that included uphill movement and stair descending as illustrated by the red dashed line in Figure 12. The pedestrian walked along the trajectory defined by the red dashed line. She stopped for a few seconds when moving uphill (blue), on the horizontal plane (red) and when descending the stair (green). The travelled distance is approximately 147 m, and the mean error is approximately 0.68% of the travelled distance.

Table 1 summarizes the final return position errors along the two trajectories. The maximum error is 1.61% during running along trajectory 1. The mean error of the two trajectories is 0.89%. The data in Table 1 indicate that the proposed algorithm provided robust positioning results under various motion modes and with different pedestrians.

Figure 11. Comparison of the calculated trajectories between the proposed algorithm and algorithm [11].

Figure 11. Comparison of the calculated trajectories between the proposed algorithm and algorithm [11].

Figure 12. Uphill and stair descending trajectory. Path used for testing (left). The calculated trajectories of the proposed algorithm (right).

Figure 12. Uphill and stair descending trajectory. Path used for testing (left). The calculated trajectories of the proposed algorithm (right).

Table 1. Field test return position errors.

Event		Average Speed (m/s)	Travelled Distance (m)	Position Error (%)
Trajectory 1	Walking	1.3	422	0.60%
	Running	2.6	421	1.61%
	Running and Walking	1.8	422	1.10%
Trajectory 2	Stop on the uphill	0.8	148	0.44%
	Stop on the horizontal plane	0.7	147	0.67%
	Stop on the stair descending	1.2	147	0.93%

Table 1. Field test return position errors.

Event		Average Speed (m/s)	Travelled Distance (m)	Position Error (%)
Trajectory 1	Walking	1.3	422	0.60%
	Running	2.6	421	1.61%
	Running and Walking	1.8	422	1.10%
Trajectory 2	Stop on the uphill	0.8	148	0.44%
	Stop on the horizontal plane	0.7	147	0.67%
	Stop on the stair descending	1.2	147	0.93%

7. Conclusions

The experimental results show that the algorithm proposed in this paper can correctly detect zero velocity intervals under various motion modes. The algorithm is insensitive to the mounting position of the IMU and differences in pedestrians.

We also conducted experiments under an elevator environment. Because the acceleration of the elevator was lower than the threshold Th_a, the ZUPT state machine remained in the zero velocity state regardless of whether the elevator was rising or descending. We will attempt to solve this problem in the future.

ZUPT cannot estimate or compensate for heading errors, which are a primary error source in PNS. Other techniques, such as received signal strength (RSS) measurements [16], map matching [2,17], building heading [7] or computer vision-derived position measurements [18] can be employed to improve the accuracy of a PNS. We will discuss the integration of these techniques with ZUPT in a future paper.