3 D Reconstruction of Pedestrian Trajectory with Moving Direction Learning and Optimal Gait Recognition

An inertial measurement unit-based pedestrian navigation system that relies on the intelligent learning algorithm is useful for various applications, especially under some severe conditions, such as the tracking of firefighters and miners. Due to the complexity of the indoor environment, signal occlusion problems could lead to the failure of certain positioning methods. In complex environments, such as those involving fire rescue and emergency rescue, the barometric altimeter fails because of the influence of air pressure and temperature. This paper used an optimal gait recognition algorithm to improve the accuracy of gait detection. Then a learning-based moving direction determination method was proposed. With the Kalman filter and a zerovelocity update algorithm, different gaits could be accurately recognized, such as going upstairs, downstairs, and walking flat. According to the recognition results, the position change in the vertical direction could be reasonably corrected. The obtained 3D trajectory involving both horizontal and vertical movements has shown that the accuracy is significantly improved in practical complex environments.


Introduction
Pedestrian navigation refers to a user achieving real-time navigation and positioning by carrying different sensors, which can transmit the movement information to monitoring personnel; thus, it is extremely significant for those personnel who need to work in an unknown or dangerous environment.As an important branch of navigation, pedestrian navigation has been paid increasing attention in various industries.
At present, technologies suitable for pedestrian navigation can be divided into two technologies, satellite-based Global Navigation Satellite System (GNSS) [1] providing global position solution and ground-based positioning using radio signals.The first kind is relatively mature, but the signals are blocked by tall buildings or when users are indoor or inside places like caves or jungles.The second kind based on the wireless frequency signal (such as wireless fidelity [2], radio frequency identification [3], and ultrawideband [4]) requires preinstallation of signal transmitting equipment in the positioning area, which is costly and has a limited application range.It cannot be used in the positioning area and in rooms where there are no signal transmitting/receiving devices.People who have dangerous occupations, such as firefighters or police officers, usually work in such situations and have a high demand for precise locations.This is because the pedestrian navigation system is used in an unknown environment.Compared with other positioning technologies based on self-contained sensors, the inertial measurement unit (IMU) has the advantage of strong independence and mainly adopts sensors, such as accelerometers and gyroscopes, to calculate pedestrian position information [5,6].
Thanks to miniaturization technologies, such as microelectromechanical systems or nanoelectromechanical systems, IMUs have become smaller, low-cost, less power consuming, and can be fixed to a pedestrian foot.The foot-mounted IMU has many indoor applications, such as in antiterrorism efforts and in providing help for fire and other dangerous situations [7].For example, in the antiterrorism application, the IMU system can provide locations to facilitate smooth conduct of police operations and indoor cooperation; this has great application value.
The main algorithm structure of the pedestrian navigation system was proposed by Foxlin [8]; this is a shoe-mounted method using the extended Kalman filter, zero-velocity updates, and a strapdown inertial navigation system (SINS).Over the past few years, some researchers have tried to add other information sources to this main algorithm structure, such as building heading information proposed by Cai et al. [9], prior maps used by Lategahn et al. [10], and visual sensors used by Flores et al. [11], to solve the problem of system error drift.However, the applications of these methods are limited to only some specific situations, and the system accuracy is affected heavily by the selected sensors.
For the 3D positioning of indoor floors [12,13], due to the drift and integral error of the IMU, the positioning results are very unstable, especially in the vertical direction.Furthermore, because of the influence of gravity acceleration, there is a great deviation in the calculation of the height displacement.To solve this problem, some researchers used the IMU to combine building structures, such as Fei et al. who proposed a method using a building heading-aided low-cost inertial navigation [14].However, firefighters often execute rescue missions where there are no prior maps.Additionally, Jiang et al. used visual tracking assists in IMU calibration to avoid the bias drift during pedestrian navigation [15]; experimental results show that the IMU and visual tracking are complementary to each other.However, visual sensors may be useless when it is dark or when the environment is full of smoke.Furthermore, Zeng et al. used UWB/IMU sensor fusion for indoor pedestrian tracking, which has the advantage of higher accuracy and stronger anti-interference capability due to the features of UWB [16].Its disadvantage is that the UWB base station must be arranged beforehand.A conventional solution uses the IMU with the barometer height to achieve vertical positioning of the stairs [17], but the barometer is easily affected by temperature flow intensity and other factors in fire and other harsh environments.
This paper used an optimal gait recognition algorithm to improve the accuracy of gait detection.Then we combined this with Kalman filter and the zero-velocity update algorithm to accurately recognize gait using our proposed moving direction learning method.According to the results of direction determination, we took different coping methods.Finally, accurate tracking between floors was obtained.The proposed method was evaluated with walking experiments and comparisons with other methods without using our method and only applying SINS and the method using the IMU and barometric pressure fusion to solve 3D pedestrian trajectory.The experimental results show the effectiveness of the proposed method.The error in the horizontal direction was less than 0.08 m per 10 m, and the error in the vertical direction was less than 0.14 m per 10 m.The main contribution of this paper is that the proposed method is independent of various kinds of sensors.It even achieved better performance than the existing multisensor fusion methods [12,18].In addition, the optimal gait recognition and the moving direction learning method will also bring much benefit to indoor positioning problems.This paper is organized as follows.Section 2 introduces the system structure of the proposed pedestrian navigation system (PNS), including hardware structure and algorithm composition.The optimal gait recognition algorithm, which plays an important role throughout the system, is discussed in Section 3. In Section 4, Kalman filter is applied for online computation of attitude transform matrix, position, and velocity.Then, the moving direction learning method is proposed in Section 5. Section 6 shows the evaluation results of two sets of experiments.In Section 7, we give the main conclusions drawn from this work.

System Structure
The PNS used in this paper only included one IMU, which was fixed on the left foot, as shown in Figure 1.The IMU adopted in our system was the MTi integrated sensor from Holland Xsens company as in Figure 2 [19].It contains a three-axis accelerometer, three-axis gyroscope, and threeaxis magnetometer, which can be connected to a computer by an RS232 interface or USB interface.The sampling rate was set as 100 Hz, and its dynamic range was ±50 m/s 2 , ± 300 deg/s, and 600 mg.
The algorithm flow of the PNS was as follows: (1) An improved zero-velocity update algorithm that could more  2 Complexity accurately determine when the IMU should be stationary and could be used as a reference in the Kalman filter to correct the predicted position, velocity, and attitude angle of SINS; and (2) a linear gait discrimination method used to judge the walking state of pedestrians and solve the inaccurate positioning problem on floor height estimation, thus realizing accurate indoor trajectory reconstruction.The algorithm architecture is shown in Figure 3.

The Optimal Gait Recognition Algorithm
3.1.Zero-Velocity Detection.Zero-velocity detection determines whether the pedestrian foot is on the ground.The detected zero-velocity state could be used as the external measurement information for the system to reduce speed error and improve positioning accuracy.One of two different states, that is, moving and static, was an output of the zerovelocity detection algorithm based on the signal source.Zero-velocity detection [20,21] plays an important role in the whole system.In this study, a generalized likelihood ratio test algorithm was used, where the equivalent moving variance of acceleration and the moving mean square value of angular velocity are used for zero-velocity detection.
The output of IMU was defined as where x a n and x w n ∈ Ω 3 are the specific acceleration and angular velocity measurement vectors, respectively and n represents the index for sampling instants when the sensor data was collected.By the Neyman-Pearson rule, we assumed the value z n = x n , x n+1 , … , x n+W−1 , where W represents the number of samples.We can get the T z n model as follows: where T z n is the test statistics as shown by the red curves in Figure 4, g represents the acceleration due to gravity, x a k and x w k represent the means of the samples, respectively, and δ 2 a and δ 2 w represent the noise variances of accelerometers and gyroscopes, respectively.
In the stage of gait division, detection threshold T d z n must be preset.In this study, we set T d z n = 0 3 × 10 5 , as shown by the blue line in Figure 4.By comparing the detection statistic T z n with the threshold T d z n , the gait detection results were obtained.Gait detection results were expressed as follows: where S k stands for the detection result, corresponding to the black solid lines shown in Figure 5.When S k equaled to 1, it indicated the static phase; when it equaled to 0, it indicated the moving phase.  3 Complexity 3.2.Improved Zero-Velocity Detection Algorithm.When the foot steps on the floor, it is actually static.However, due to the drift of the measurement data of IMU, the data shows that the foot is moving.Therefore, we call this phase the pseudomoving phase, which can be induced by the local uplift fluctuation of the static phase and usually lasts a short time.The test result is shown in Figure 5.The improved zerovelocity detection algorithm added a preset time threshold T l on the basis of the generalized likelihood ratio detection algorithm.The improved zero-velocity detection results were expressed by the following formula: where L T z n > T d represents the duration of the gait phase.The modified gait detection result is shown in Figure 6.Traditional zero-velocity detection results contain a lot of pseudomoving phases, which results in inaccurate location calculations.The improved gait detection algorithm segmented the gait effectively.

Position Estimation Based on Kalman Filter
After getting the gait detection result, we needed to estimate and correct the location of pedestrians through the zerovelocity reference combined with the Kalman filter algorithm.
4.1.Calculation of Attitude Transform Matrix.Data in body coordinate b and navigation coordinate n meets the following equation: where the attitude transform matrix C n b represents the rotation relationship between the two coordinates, x n , y n , and z n represent the geographical coordinates east, north, and up, respectively, while x b , y b , and z b indicate the forward, left, and upward directions in the body coordinate system.
Therefore, we could get the acceleration f of the navigation system as follows: where accX, accY, and accZ represent the acceleration along three axes in the body coordinate system.Besides, f can be expressed as f 1 f 2 f 3 T , which represent the acceleration along the x n -, y n -, and z n -axes, respectively.Based on the acceleration data in the navigation coordinate, we could obtain the pedestrian location and speed information via integration computation.Therefore, the accuracy of C n b , acc X, accY, and accZ were very important for accurate pedestrian trajectory reconstruction.The method of calculating the attitude matrix by quaternion has been discussed by many researchers [22], and thus, we just list them as follows: Considering the relation between quaternions and gyro measurement data in the body coordinate system, the updated equation of quaternions is as follows, and we suppose the quaternion is set as where w x , w y , and w z represent the x-axis, y-axis, and z-axis angular velocity, respectively.According to (8), the quaternion matrix was obtained through the Picard approximation method: Δθ 0 Δθ q 0 , 10 4 Complexity where q 0 is the initial quaternion and Δθ denotes the change in angle, which can be obtained as follows:

11
where Δθ x , Δθ y , and Δθ z are changes in angles and can be obtained by Δθ x = w x × Δt, Δθ y = w y × Δt, and Δθ z = w z × Δ t, respectively.Then the attitude transform matrix C n bk can be expressed in the form of quaternions, as shown in 2 q 2 0 + q 2 1 − 1 2 q 1 q 2 − q 0 q 3 2 q 1 q 3 + q 0 q 2 2 q 1 q 2 + q 0 q 3 2 q 2 0 + q 2 2 − 1 2 q 2 q 3 + q 0 q 1 2 q 1 q 3 + q 0 q 2 2 q 2 q 3 − q 0 q 1 2 q 2 0 + q 2 3 − 1 12 4.2.Prediction of Pedestrian Position and Velocity.Since the IMU will have a certain drift and error will be accumulated over time, the Kalman filter was applied here to correct the distortion.In a regular walking cycle, the foot-mounted IMU will periodically swing.Using the gait detection results, the Kalman filter could estimate the errors of attitude, velocity, and position effectively.The model of zero-velocity correction for the Kalman filter was constructed as follows [23]: where x is the system state vector, which is defined as x = δs x δs y δs z δv x δv y δv z δρ δξ δψ T 14 In the expression above, δs x , δs y , and δs z represent the position errors of the target, δv x , δv y , and δv z are the velocity errors of the target, δρ, δξ, and δψ are the errors in attitude angles.w is the process noise with covariance Q w .
F in ( 13) is the state transition matrix with 9 rows and 9 columns: and F m is structured as follows: Formula (13) introduces a continuous state model, and the discretized equation is as follows: where A = I + FΔt + 1/2 FΔt 2 , x k represents the state of time instant t k , and w k represents process noise.The covariance of w k is calculated by Q k as , where G k is a matrix with 9 rows and 6 columns: The observation model is where z k is the measurement matrix and τ k is the white Gaussian noise.H is the measurement matrix: H = 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 21 Based on the output of IMU and the estimated state variable at time t k−1 , the velocity and position were calculated with the acceleration f in the navigation coordinate obtained by (6) as follows:

22
where the v k matrix is the velocity at time t k and v k = v x , v y , v z .The s k matrix is the position vector at time t k and s k = s x , s y , s z .v k−1 and s k−1 are the estimated velocity and position at time t k−1 , respectively.Δt is the sampling time.It is known that v k and s k will gradually drift away from the true value due to the error of the measured accelerations accX, accY, and accZ.

Update Based on Kalman
Filter.After the Kalman filter predicted the velocity and position, the zero-velocity information was used to correct the velocity.When the foot was stationary, we used the measurement data Then using the Kalman filter, we update the current state as follows: where xk,k−1 and xk,k are the estimated states updated by the 5 Complexity measurement z k .K k is the filter gain of the Kalman filter, which can be calculated by The error covariance of the state estimation is calculated by The error covariance of the update process can be computed by where k, k − 1 refers to the predicted value from the sampling k − 1 to k, k, k refers to the k point estimates, and P k,k−1 is the step prediction covariance.We set the prediction error d as d = xk,k − xk,k−1 , where d is a 9-dimensional vector and can be described Then the updated position and velocity are calculated as follows: where s k is the position corrected at time tick t k and v k is the velocity corrected at time tick t k .d 1 − 3 is the first, second, and third elements of d; d 4 − 6 is the fourth, fifth, and sixth elements of d; and λ is denoted as the coefficient of deviation.By adjusting the value of λ, the errors of position and velocity could be adjusted.It needs to be pointed out that λ is dependent on the step length, which is related to the height of the pedestrian.
In order to correct the rotation matrix, the correction matrix Pe was constructed as follows: With the above matrix, the transform matrix can then be updated via the following equation: where β is the coefficient closely related to the step length of people and step length is usually proportional to the height.The average height of adults is 1.5 m to 2 m; therefore, the range of the parameter β is set to be 1.5-2.

Moving Direction Learning Method
In order to get the exact vertical height, we presented a linear discriminant function based on the pedestrian gait.First, we judged whether the pedestrian was climbing the stairs or walking on the flat ground; then, if they were climbing the stairs, we judged whether they went up or down the stairs.According to the results of different judgments, we presented a linear discriminant function based on the pedestrian gait, by which the walk pattern of every step, such as up or down the stairs, can be detected.Our specific methods are as below.
The step size calculation formula is defined as follows: where d xy is the displacement of each step, x end and y end are the surface coordinates of the last point during the moving phase, and x start and y start are the first points during the moving phase.
According to the actual measurement, when an adult walks, the horizontal displacement of each step is 0.9 m-1.15 m.Meanwhile, when taking a step up or down, the horizontal displacement is 0.3 m-0.38 m; taking two steps up or down, the horizontal displacement is 0.6 m-0.75 m.It can be seen that the two scenes are very different in step size.We set the threshold d s = 0 8 m, by comparing horizontal displacements, to distinguish between taking the stairs and walking on a flat plane.
If the direction of vertical movement is necessary, some further rules need to be investigated as well.Here, the z-axis displacement was calculated by using SINS.As we all know, the vertical displacement cannot be obtained directly by integrating the z-axis acceleration due to the drift of IMU data.However, through a large number of experiments, we found out that although there will always be a drift, the calculation results can accurately reflect the going up or down the stairs.Therefore, the difference between the upper and lower position d z was computed as follows: where z start and z end are the coordinates in the vertical direction at the start and end points that are the same as those in (31).In summary, the linear gait discrimination function can be obtained as follows: If it is judged that a staircase was climbed, the threshold was further set to judge whether one or two steps were climbed at a time; we set another threshold as d sl = 0 5 m.If d z > d sl , then the z coordinate of the end point is assigned z start plus twice the stair height, that is, z end = z start + 2h.If d z < d sl , then the z coordinate of the end point is assigned z start plus the stair height, that is, z end = z start + h.If the pedestrian went downstairs, then z end = z start − 2h or z end = z start − h; this is similar to going upstairs.If it was judged that the pedestrian was walking on a flat plane, then we set z end = z start .

Complexity
The h value needed to be calculated at the first step of the stairs.At the beginning of the test, the stair height calculation was accurate.Because the drift was weak, acceleration could be used for position computation in the vertical direction.Besides, with the relationship between z end and z start , the value h could then be determined.If one step was climbed at a time, then h = d z1 = z end1 − z start1 ; if two steps were climbed at a time, then 2h = d z1 = z end1 − z start1 .Here, d z1 is the height of the first step of the staircase.The first step height h was used in each subsequent calculation.
The verification result of the proposed stair height calculation method is shown in Table 1.Three different bench heights were chosen as 0.15 m, 0.16 m, and 0.165 m; each group was verified by 20 sets of experiments; and the average stair height was used as the h value.

Case Study
In this section, two different cases were discussed to illustrate the effectiveness of the developed method.In Case 1, positioning in the 2D plane was considered and a reasonably accurate result was derived.Furthermore, a 3D scenario was implemented in Case 2, where the performance of the linear gait discrimination method was verified.
Case 1.The experiment was conducted in the corridor of Number 3 Teaching Building in Beijing Technology and Business University, as shown in Figure 7.After using the optimized zero-velocity detection algorithm and selecting the appropriate parameters, the positioning results of the 2D horizontal plane were shown in Figure 8.The actual walking distance in the corridor was 39.6 m, while the calculated value was 39.95 m.In this case, the experimental error was 0.35 m.The accuracy of the horizontal experiment results were verified by Case 1.
Case 2. In Case 2, the linear gait discrimination method was used to obtain an accurate 3D indoor trajectory reconstruction.The experiment was carried out in the stairwell in the building, as shown in Figure 9.

Complexity
Pedestrian attitude discrimination experiment.Five people of different height, weight, and sex were selected to carry out this experiment.The heights of the test personnel were 160-185 cm.During the test, the testers walked randomly up/down the stairs and along flat planes.They were asked to keep a real walking posture.The actual pedestrian walking posture and gait discrimination results were compared, and the comparison results are shown in Table 2.It could be clearly seen that the error rate of the algorithm was less than 1.35%.The results showed that the linear gait discrimination method proposed in this paper had a high recognition rate.In the table above, the error rate was calculated by the formula where r e is the error rate, l e is the wrong number of steps, and L is the total steps walked, and the number of actual walked steps was equal to the number of test steps.
Validation of height estimation algorithm.The subject started from the sixth floor staircase, climbed to the ninth floor, and then walked a distance in the corridor.The actual floor height was 3.5 m, and with three stories, this resulted to 10.5 m in total.We carried out three experiments: Experiment 1, without using our method and only applying SINS; Experiment 2, using the linear gait discrimination method; and Experiment 3, using the IMU and barometric pressure fusion to solve the 3D pedestrian trajectory.All the starting points were 0, 0, and 0. The sampling frequency of the barometer was 0.01 s, and the formula for calculating the height of the barometer was as follows: where H is the current height value of the calculation, p is the barometric value at a specific moment, and p 0 is the initial barometric value.Then the IMU and barometer results were calculated by the fusion method [23].The measured height of the barometer was 10.74 m.The result of Experiment 1 was shown in Figure 10, and the results of Experiments 2 and 3 were given in Figures 11  and 12, respectively.To better show the different performances of these methods, a detailed comparison result was given in Table 3.It could be clearly seen that the error in Experiment 1 was large, up to 3.5 m, and the proposed method also behaved a bit better than the IMU/barometer fusion method with the error of 0.14 m only.The above three experiments were carried out 30 times in each group, and the errors of Experiment 2 and Experiment 3 were within 0.3 m per 10 m.However, in an environment such as in a fire rescue, the barometer will fail, and therefore, the combination of the barometer and IMU is used to increase the computational complexity.The improved     8 Complexity method using only the IMU not only overcame the redundancy of multisensor fusion but also completed the 3D trajectory reconstruction.

Conclusions
The foot-mounted IMU-based pedestrian indoor navigation is useful for various indoor applications, such as finding and rescuing firefighters or other emergency first responders.This paper presented an optimal gait recognition algorithm to obtain more accurate gait detection results.Then the Kalman filter was combined with the zero-velocity update algorithm to accurately recognize gait using our proposed moving direction learning method.The proposed method was evaluated with walking experiments, and comparisons of results illustrated the effectiveness of the proposed algorithm.The error in the horizontal direction was less than 0.08 m per 10 m, and the error in the vertical direction was less than 0.14 m per 10 m.In future work, we will study more complicated movement cases, such as jumping and walking backwards, as well as movement via elevators.

Figure 1 :
Figure 1: The IMU fixed on the shoe.

Figure 11 :
Figure 11: 3D positioning results of the proposed linear gait discriminant method.

Figure 12 :
Figure 12: Results of data fusion using the IMU and barometer.
Read IMU data a x , a y , and a z w x , w y , and w z

Table 1 :
Performance of the stair height calculation method.

Table 2 :
Comparison of gait discriminant results among different pedestrians with actual walking posture.

Table 3 :
Comparison of the three groups of experiments.