Abstract
The foot-mounted pedestrian navigation system (PNS) that uses microelectromechanical systems (MEMS) inertial measurement units (IMUs) to track the person’s position. However errors accumulate over time during inertial navigation solutions, which affects the positioning precision. In this paper, a multicondition zero velocity detector is used to detect the stance phase of gait. Then the errors are corrected in the stance phase and the swing phase, respectively, through the Kalman filter. When pedestrians are going up and down the stairs, the divergence of height will reduce the accuracy of three-dimensional positioning. In this paper, an accelerometer and a barometer are used to obtain altitude variation, and after that the stair condition detection (SCD) algorithm is proposed to correct the height of Kalman filter output and detect the walking state of pedestrians. Through theoretical research and field experiments, these algorithms are studied carefully. The results of the experiment show that the algorithm proposed in this paper can effectively eliminate errors and achieve more accurate positioning.
1. Introduction
With the development of smart city, people’s demand for location service is getting higher and higher. People spend most of their time indoors, but now commercial positioning technology is mainly applied to outdoor environment, the positioning accuracy in the indoor will be decreased. Therefore, how to use the low-cost positioning system to achieve high precision positioning has become a hot research topic for researchers.
According to the literature [1], current indoor positioning solutions can be divided into two categories: One is wireless communication positioning which relies on preinstalled installations like wireless (Wi-Fi) [2], Bluetooth [3], infrared [4, 5], Ultra-Wide Band (UWB) [6, 7], etc. The second method is based on the MEMS inertial measurement unit (IMU) to form an inertial navigation system (INS) for pedestrians to track trajectories [8, 9]. A MEMS is composed of a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer, and it is installed on the human body (usually on the foot) to obtain foot motion data information. The input of pedestrian inertial navigation algorithm is acceleration and angular rate, and no additional data is required, the system complexity is low [10]. However, MEMS sensor has drift and noise, which will lead to the continuous accumulation of errors over time.
The most common method to eliminate error is zero velocity update technology (ZUPT). The most important step to use ZUPT algorithm is to detect the stance phase of the gait. The literature [11] uses the accelerometer or the gyroscope output to detect the stance phase. In the literature [12], the zero velocity detection was formalized as a binary hypothesis testing problem and a likelihood ratio test (LRT) detector was proposed. However, the most commonly used algorithms are the use of accelerometer and gyroscope output signals, the experiment is limited to simple straight lines and the pace of stability. Other scholars use other sensing devices to assist detection, such as piezoelectric sensor devices, electromyography. The accuracy is higher than the use of traditional IMU, but to pay a greater cost. In this paper, acceleration movement variance detection, acceleration amplitude detection, and angular rate energy detection are combinedly used for the detection of zero velocity intervals. This method can solve the problem of zero velocity intervals misjudgment due to the change of step speed, turning, and foot abnormal movement. The accuracy and practicability of pedestrian navigation algorithm can be further improved.
The ZUPT algorithm cannot correct the heading error due to poor observability [13]. In order to limit the accumulation of heading error, a new gyroscope drift reduction method is proposed [14], which combines the heuristic drift reduction method with the supplementary filter. The literature [15] is proposed to estimate the direction with two stages of Kalman filtering, the accuracy of the roll angle and the pitch angle reaches , and the accuracy of the yaw angle reaches . In the literature [16], the measurement variance of the sensor can be adjusted according to the gait type by using the EFK-HDR-CHAIN-FGH directional estimation algorithm. The literature [17] reduces adversity drift by means of multisensors fusion and dual gait analysis. The literature [18] uses a magnetometer sensor as a compass to estimate the heading drift error. However, magnetometers are susceptible to the surrounding environment which can lead to a decrease in the accuracy of heading estimation. In this paper, the zero angular rate update (ZARU) and the heuristic heading drift estimation method based on the main direction angle (MHDE) are proposed to correct the heading error. ZARU method mainly uses the difference between angular rate output and zero angular rate in the stance phase as observations of the Kalman filter to correct the heading error. The HDE is based on the fact that most of the walls and corridors in a building are made up of straight lines and right angles and use only four or eight dominant directions as a reference without prior information about the environment, which can effectively reduce the heading drift. The difference between the calculated heading angle and the main direction angle is corrected as the observation value of Kalman filter.
In order to obtain more accurate positioning results, it is not enough to correct errors only in the stance stage. The errors in the swing phase also need to be corrected. Therefore, this paper not only corrects the errors of the stance phase, but also corrects the errors in the swing phase.
The existing research has obtained good positioning results on the two-dimensional plane, but in the three-dimensional positioning, the calculation of height is still a key problem in this field [19, 20]. In the literature [21], the peak characteristics of accelerometer and the interquartile range characteristic are used to determine the upper and lower floors, respectively, then the height calculation of pedestrian step height is calculated by using a barometer. The barometer is susceptible to ambient temperature and has low reliability. The literature [22] estimates the change of height by combining the detection of ascending and descending stairs with the height of stairs. If the height of the stair steps in real life is inconsistent, it will lead to inaccurate height estimation. In this paper, the height of the barometer and the height of the accelerometer are fused by Kalman filter to obtain altitude variation. Subsequently, the SCD algorithm is used to modify the output height of Kalman filter and get the final height information.
The remainder of this paper is organized as follows. The second part mainly introduces the solution process of inertial navigation and several methods to reduce integral drift. The third part verifies the proposed algorithms through concrete experiments. The fourth part summarizes the content of this paper and the prospect of future work. The overall framework of this article is shown in Figure 1.
2. Error Model
2.1. Inertial Navigation Model
Inertial navigation algorithm will involve different coordinate reference systems, where the carrier coordinate system (b-system) is connected with the carrier, its origin in the carrier’s center of gravity. The carrier coordinate system is represented by , represents forward along the carrier horizontal axis, represents the vertical axis of the carrier to the right, and represents upward along the carrier vertical axis. The navigation coordinate system (n-system) is used to determine navigation parameters, commonly expressed by , which can also be called a geographic coordinate system. The origin of the n-system coincides with the measuring center of the sensor. This paper uses east-north-day as the navigation coordinate system, points north, points east, and points to the sky. The carrier coordinate system and the navigation coordinate system are shown in Figure 2.
The initial heading angle is set to 0, and the roll angle and pitch angle are calculated by the average acceleration of the first 20 moments measured by the inertial sensor. The initial attitude angle can be obtained by the following equation:where are roll angle, pitch angle, and heading angle, respectively, , , .
The quaternion is initialized by the following equation:
The data measured by the gyroscope and accelerometer are the angular rate and acceleration under the carrier coordinate system, and the acceleration information under the carrier coordinate system needs to be converted into the navigation coordinate system through a conversion matrix, which is called the attitude matrix. The corresponding attitude matrix [23] is represented by the following equation:
At the same time, the three attitude angles can be obtained by the following equation:where is the roll angle, is the pitch angle, and is the yaw angle.
According to the quaternion differential equations, the attitude matrix can be updated in real time, and the quaternion differential equations is as follows:where , , so equation (3) can be written as the following equation:
The Picard algorithm is used to update quaternion as the following equation:where , and can be obtained by the following equation:
, , are the angle increment of the gyroscope. After the attitude is updated, the acceleration vector of the b-series is converted into the acceleration vector of the n-series through the following equation:
The acceleration of gravity is subtracted from the acceleration of the n-series to obtain the pedestrian’s own motion acceleration value. The velocity and position can be updated by integral of acceleration and double integral of acceleration, respectively. The velocity and position are obtained according to the following equation:
Because of the serious drift and noise of the inertial sensor, the position obtained by integration will have a large error. Therefore, corresponding method must be taken to correct the errors.
2.2. Gait Analysis
The movement of pedestrians in the process of walking is periodic. During each step of horizontal walking, the velocity repeats from stationary, accelerated, decelerated, and then stationary again. In this paper, a gait cycle is divided into two stages: the stance phase and the swing phase. The stance phase foot is completely on the ground, while the swing phase is to swing from the foot away from the ground in the air until the heel comes into contact with the ground again. The velocity of the foot is obviously zero in the stance phase so that the velocity can be corrected periodically to reduce the error accumulation due to the bias error of accelerometer, which would otherwise trouble the data processing in the following strides.
Because everyone has a unique gait pattern, the percentage of gait cycles spent at each stage is slightly different. If an adult walks at a normal pace of 100–120 steps per minute, the stance phase will last around 0.3 seconds, which is considered sufficient for the ZUPT algorithm to correct navigational errors.
In order to accurately detect the stance phase of pedestrians, acceleration and angular velocity values must be fully utilized. The information of acceleration and angular velocity during walking is shown in Figures 3 and 4. Because the sensor is mounted on the foot of the pedestrian, the original data in the figure also shows the same periodic characteristics. In Figure 3 it can be seen in the stance phase that acceleration and angular velocity are kept relatively stationary. In the stance phase, z-axis acceleration fluctuates around , which is caused by gravity acceleration. And x-axis and y-axis acceleration are near . In Figure 4 it can be seen that the values of angular velocity tend to be zero. The values of the acceleration and angular velocity are unstable in the swing phase. The acceleration and angular velocity are obviously different in the stance phase and the swing phase. So the values of acceleration and angular velocity can be used together for the stance phase of detection.
2.3. Stance Phase Detect
In this paper, the acceleration amplitude detection and angular velocity energy detection are added on the basis of the commonly used acceleration moving variance detection [24], which adds the constraints of zero speed detection. According to the experiment and related literature, acceleration variance detection for normal linear walking detection results is better. Acceleration amplitude detection for large walking and fast walking situation detection results is better. Angular velocity energy detection mainly uses the output data of the gyroscope, and the zero velocity detection effect of the angular rate value mutation caused by the left and right shaking of the foot and the turning process is obvious. Therefore, the zero velocity detection algorithm proposed in this paper can make full use of the data of accelerometer and gyroscope to detect zero velocity interval more accurately.
Acceleration variance detection is to build a sliding window based on acceleration, which traverse the data along with the acceleration information in order to obtain the acceleration movement variance:where is the ratio force measurement, and is the mean ratio force.
Acceleration amplitude detection:
Angular velocity energy detection:
When the output data of the accelerometer and gyroscope meet the above three detection conditions at the same time, it is determined to be the stance phase of the gait, and the formula is expressed as follows:
When is the stance phase, is the swing phase; the results of zero velocity detection are shown in Figure 5.
2.4. Stance Phase Error Correction
During the stance phase, the foot of the pedestrian is in full contact with the ground and remains stationary for a short period of time, but the output of acceleration and angular velocity is not zero due to the drift error of the sensor itself. The difference between the sensor output velocity and the zero velocity is considered a velocity error, which can be corrected as an observation of the Kalman filter. The same principle can be used to correct the value of angular rate. In this paper, the velocity and heading of the stance phase are corrected by using the Zero Velocity Update Algorithm (ZUPT) and the Zero Angle Rate Update Algorithm (ZARU).
2.4.1. Velocity Correction
In this paper, the velocity is corrected by ZUPT algorithm during the stance phase. The difference between velocity and zero velocity in the stance phase is used as the observation value of Kalman filter to correct the velocity of the stance phase.
Kalman filtering [25] is a common algorithm used in inertial navigation systems to compensate for cumulative errors. The state variables selected when using ZUPT in stance phase are , is the position error, is the velocity error, is the attitude error, is the accelerometer drift, and is the gyroscope drift.
The state equation in the inertial navigation system iswhere is the state transfer matrix, is the process noise matrix, the Gauss process noise, and the covariation matrix is . Assuming the sample time is , the state transfer matrix iswhere , are correlation time.
is the skew symmetry matrix of acceleration, whose elements are composed of the acceleration converted to the n-system. can be written as follows:
The process noise driven matrix is
The observation equation iswhere is observation, , is measurement noise, which is white Gaussian noise with covariance matrix , and is measurement matrix. When the velocity error is used as observation, the observation matrix can be obtained by the following equation:
Kalman filtering process consists of two main update processes, time updates and status updates. When a pedestrian’s foot is in swing phase, only a time update is made to estimate the error of the covariance matrix :
When the zero velocity interval is detected, a status update is made:and the filter gain is updated as
Finally, update the covariance matrix:
2.4.2. Heading Correction
The drift error of the gyroscope is another major problem affecting the positioning results, but due to the poor observability of the heading angle, the drift of the gyroscope cannot be corrected by the ZUPT. On the basis of not adding other sensors, this paper uses the ZARU [26, 27] to correct the heading error.
In the stance phase, not only can the velocity be corrected by the ZUPT, but also the heading can be corrected by the ZARU. The angular velocity output value in the stance phase is used as the observation value of Kalman filter, which inhibits the drift of angular velocity and improves the accuracy of pedestrian navigation system. Because the feet in the stance phase are stationary relative to the ground, the attitude is theoretically unchanged, so the output of the angular velocity should be zero. The output of the angular velocity in the stance phase can be used as the error observation for the Kalman filter. The observation of ZARU in the stance phase is
Combining ZUPT and ZARU, the measurement matrix is
In order to further improve the accuracy of heading, a heuristic heading drift estimation method based on the main direction angle (MHDE) is proposed.
HDE algorithm only uses gyroscope data when calculating heading angle. In addition, the system is sensitive to the feedback coefficient used to correct the angular rate, which leads to the poor robustness of the system. MHDE deals with the heading angle directly. The difference between the calculated heading angle and the main direction angle is used as the observed value of Kalman filter to correct the heading angle.
Generally, the main directions are divided into four or eight. In this paper, the main directions are divided into four, the interval between the main directions is , and the initial main direction is set to .
When the pedestrian walks in a straight line, the difference between the heading angle of two adjacent steps is very small. When the pedestrian turns, the difference between the heading angle at the previous step and the current step is relatively large. Therefore, the turning movement of pedestrians can be detected by the mutation of heading angle between two adjacent steps. In order to ensure the correctness of turning detection, this paper uses the change of heading angle of three adjacent steps to detect turning activity. Turning activity is detected according to the following equation:where represents the walking state of the pedestrian, when represents the pedestrian walking in a straight line, and when represents turning motion. represents the heading angle of the current step, , represents the heading angle of the previous step and the first two steps. is the deviation threshold, which is set as in this paper.
When the pedestrian walks along a straight line, the difference between the heading angle of inertia solution and the main direction angle is taken as the observed value of the Kalman filter.
The observed value is .where is the heading angle calculated by inertial solution process, and is the main direction angle closest to the heading angle.
When MHDE method is used for heading correction, the measurement matrix is
2.5. Swing Phase Drift Correction
In order to further improve the accuracy of positioning, it is not enough to only use the error correction in the stance phase. Therefore, this paper not only corrects the error of the stance phase, but also corrects the error of the swing phase [28]. Unfortunately, the methods used in stance phase can not be applied in swing phase because there is nothing stationary in the swing phase.
In most cases, although the beginning of the swing phase is set to zero with ZUPT, the velocity at the end of the swing phase is not zero. During the swing phase, the velocity is obtained by the integral of acceleration, and the attitude angle is obtained by the integration of the angular rate. However, the output data of MEMS has random noise and temperature drift. The attitude angle and velocity obtained by integral have significant errors. It is assumed that this situation is caused by a fixed bias . The acceleration value in the swing phase can be expressed as follows:where is the acceleration measurement and is the actual acceleration.
Because the velocity of the stance phase is zero, the velocity of the swing phase is
The first part is the actual velocity, and the second part is the velocity error caused by the bias of the accelerometer. is zero when the time is equal to the duration of the swing phase. So the velocity error during the swing phase can be expressed as follows:where is the end time of swing phase.
On the other hand, the attitude angle is susceptible to integral drift, which results in attitude angle error. Similar to velocity correction, it is assumed that the error is caused by a constant bias of the gyroscope.where is the measurement of the angular rate, and is the actual value of the angular rate. The true value of the angular rate at the end of the swing phase should be zero, so the error of the angular velocity can be expressed aswhere is the end time of swing phase.
The error vector of the error correction at the end time of the swing phase is
The measurement matrix is
Finally, the velocity error is subtracted from the velocity of each moment of the swing phase to obtain more accurate velocity and displacement. When the attitude matrix is recalculated, the estimated bias is subtracted from the angular velocity.
2.6. Height Calculation
In real life, people’s activity trajectory includes not only two-dimensional horizontal walking, but also three-dimensional vertical motion. Pedestrians’ gait when walking up and down stairs is more complex than that of horizontal walking [29, 30], the inertial sensor will have more serious noise. In this paper, a barometer is added on the basis of the inertial sensor. The barometer is susceptible to ambient temperature, so the height calculated by the barometer and the height calculated by the accelerometer are fused by the Kalman filter. However, the output height of Kalman filter can still observe some height fluctuations. Therefore, based on the height of Kalman filter output, the SCD algorithm is proposed to modify the height of Kalman filter output. This algorithm can effectively maintain the stability of the same floor height and further improve the measurement accuracy.
The absolute height calculated by the barometer can be obtained by the following equation:where is the barometer value for the current moment.
The absolute height of the pedestrian can be calculated, but the absolute height does not provide the height position of the pedestrian, and the relative height needs to be calculated by the following equation:
The height of the accelerometer can be obtained by the secondary integral of acceleration.
The height update algorithm proposed in this paper estimates the difference between the height of the barometer and the height of the accelerometer in the adjacent moment as the observation of Kalman filtering, which can optimize the height in the adjacent moment and thus improve the height estimation. The measurement error vector of this method is , which can be get from the following equation:
is the height estimated by the barometer, and is the height estimated by the accelerometer.
The error vector for the height update algorithm is set to
The measurement matrix is
The height of the Kalman filter output is corrected by the SCD algorithm.
The SCD algorithm can be described as follows:where is the detection threshold, and is the detection result of ascending and descending stairs. If the height difference between two adjacent steps is greater than the negative value of the threshold, the pedestrian is considered to be ascending stairs; if the height difference between two adjacent steps is less than the threshold, the pedestrian is considered to be descending stairs; otherwise, the pedestrian is considered to be walking on flat ground.
When a pedestrian walks on flat ground, the height of the current step is equal to the height of the previous step; when a pedestrian goes upstairs, the current height is equal to the height of the previous step plus the height of the stair; when a pedestrian goes downstairs, the current height is equal to the height of the previous step minus the height of the stair. Then a new height is output by using this method.
3. Experiment
To verify the validity of the methods proposed in this paper, we conducted a series of experiments using MTI-G-710 from XSENS of the Netherlands. The sensor module can output three-axis acceleration, three-axis angle speed, and three-axis attitude angle (pitch angle/roll angle/yaw angle). It has small size, light weight, wide measurement range, and low price, which can fully meet experimental requirements. Other related properties of the MTI-G-710 are shown in Table 1. In the experiment, the inertial sensor module is mounted on the back of the foot as shown in Figure 6, and it is connected to the computer through a USB cable. The computer is placed on the hand of pedestrian to collect inertial data when pedestrian walks.
3.1. Two-Dimensional Plane Experiment
In order to verify the validity of the method proposed in this paper, pedestrian walks a circle along the square path and finally returns to the origin. The edge length of the square track is . The inertial sensor is mounted on the foot to collect raw data during walking, as shown in Figure 7. Because of the tilt of the sensor module, the average acceleration of the stance phase is not actually zero. These tiny offsets which are close to zero are part of gravity itself. During the stance phase, the angular velocity of each axis is close to zero.
(a)
(b)
Figure 8 shows the results of the stance phase detection using the ZUPT method proposed in this paper. The blue line indicates the velocity of pedestrian walking, the red line is the stance phase detection results, the red line value of 1 represents the stance phase, the 0 part is the swing phase. There was a false detection at the beginning of the walk due to the unsteadiness at the beginning of the walk. The stance phase at the other times can be accurately detected, which is enough to make error correction.
Figure 9 shows the pedestrian positioning trajectory on a two-dimensional plane in different ways when walking along a square path. The red dot is the starting position, the black line is the trajectory of the pedestrian’s real walk, the blue line is the trajectory of the velocity correction only with ZUPT, and the red line is the trajectory of positioning using the method proposed in this paper. Compared with the algorithm of ZUPT, the algorithm proposed in this paper adds heading correction and error correction in the swing phase. The trajectory using only the ZUPT algorithm deviates greatly from the real trajectory, and the position at the end does not coincide with the position at the beginning. Using the error correction method proposed in this paper, the corrected trajectory is closer to the real trajectory and the deviation of the direction is smaller. Table 2 shows the comparison of error results of different methods in two-dimensional plane. The overall walking distance is , the end position of the trajectory using only the ZUPT algorithm is from the starting point, and the error is 1.5% of the total walking distance. The end position of the trajectory by proposed method is only from the starting point, only 0.29% of the total walking distance.
Figure 10 shows calculated heading angles with and without heading drift compensation. Heading angle without heading drift compensation deviates significantly from the real heading angle, and the final heading angle accumulation error reaches . After using the heading correction, the heading angle fluctuates near the real heading angle, and the final heading angle accumulation error is only .
All the results prove that the algorithm proposed in this paper can get more ideal positioning results.
3.2. Stair Experiment in 3D Space
The effectiveness of the three-dimensional height correction method proposed in this paper is verified by the experiment of multistair walk. In pedestrian walking path as shown in Figure 11, the red arrow indicates the direction of pedestrian walking. Pedestrian walks along the corridor on the first floor of the Science and Chemical Building of University of Science and Technology Beijing to the stairwell, then from the first floor to the second floor, then along the corridor to the stairwell, and finally from the second floor to the first floor back to the initial position. From the first floor to the second floor, there are a total of 27 steps, the height of each step is 0.15 metres, and the total height is 4.05 metres. Figure 12 shows that the value of the measured barometer is unstable and volatile. Figure 13 shows the multistair tracks obtained by three different methods. The blue track is the track obtained by the barometer height. Due to the fluctuation of the height, the deviation of the track obtained is more serious. The green track is obtained by fusing the height of the barometer and accelerometer with the Kalman filter. The track deviation has been greatly improved, but there are still some height fluctuations on flat ground. The red track is obtained by Kalman filter and the SCD algorithm, and the height fluctuation on the 2D plane is significantly improved. This method has higher rated position accuracy and stronger stability.
In this paper, Kalman filter and the SCD algorithm are used to estimate the height change in 3D trajectory. Figures 14–16 show the height estimation of three different height measurement schemes in the process of multistair movement. Table 3 shows the results of height errors obtained by the three schemes. Figure 14 shows the height measured by the barometer. Due to the instability of the barometer, the height measured by the barometer is irregular. The maximum error reaches . In Figure 15, the Kalman filter was used to fuse the height of the accelerometer and barometer, and the height fluctuation was significantly reduced, with a maximum error of . But there are still fluctuations in height when walking on flat ground. In Figure 16, the height obtained based on Kalman filter and SCD algorithm is more stable and accurate. In particular, the height on the plane is obviously better than the height obtained by the other two schemes. The maximum error is only
It can be seen that the trajectory of the height correction algorithm proposed in this paper is closer to the real trajectory and has better performance in three-dimensional pedestrian navigation and positioning.
4. Conclusion
In this paper, inertial sensor is used for pedestrian inertial navigation; this method will reduce the accuracy of positioning results due to the drift error of low-cost inertial sensors. In this paper, several algorithms are proposed to reduce the integration drift in the process of inertial solution. Firstly, the stance phase of the pedestrian gait is detected by a multicondition zero velocity detector, and then the ZUPT method was used to correct the velocity of the stance phase. The ZARU method and the MHDE method were used to correct the heading of the stance phase. In order to further improve the accuracy of positioning, the error correction of the swing phase is added. The error correction method proposed in this paper is verified by the square trajectory on the two-dimensional plane, and the final positioning error is 0.29%. It is shown that the error correction method of this paper can achieve relatively accurate positioning results. For 3D positioning involving stairs up and down, the height needs to be accurately estimated. In this paper, the Kalman filter and the SCD algorithm are used together for height estimation. The validity of the height correction algorithm proposed by this paper is verified by three-dimensional staircase experiment, and the final height error is only .
Because the walking state of pedestrians is complex and random, gait detection algorithm can be designed to detect pedestrian walking state in future research. In this paper, the inertial sensor is mounted on one foot, the sensor can be installed on both feet, the data of both feet is analyzed, and the stability and accuracy of the pedestrian positioning system can be further improved.
Data Availability
The simulation experiment data used to support the findings of this study are available from the author (Xiaomeng Wu; [email protected]) upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported by the National Key Research and Development Program of China under Grant 2017YFF0207400. The authors would appreciate the supports and funds.