An Improved Particle Filter Map Matching Algorithm for Personal Inertial Positioning

Xiaolong Zhang¹, Tao Zhou¹, Jing Wang¹, Tao Wang¹, Hui Zhao²

¹State Grid Shanxi Province Electric Power Company Yangquan Power Supply Company, Yangquan, China.
²School of Automation, Beijing Information Science and Technology University, Beijing, China.

DOI: 10.4236/jcc.2023.116007                

Abstract

The current particle filtering map matching algorithm has problems such as low map utilization and poor accuracy of turnoff positioning, etc. This paper proposed an improved particle filtering-based map-matching algorithm for the inertial positioning of personnel. The historical moment position constraint and feasible region constraint of particles were introduced in this paper. A resampling method based on multi-stage backtracking of particles was proposed. Therefore, the effectiveness of newly generated particles could be guaranteed. The utilization rate of map information could be improved, thus enhancing the accuracy of personnel localization. The walking experiment results showed that, compared with the traditional PDR algorithm, the proposed method had higher localization accuracy and better repeatability of the localization trajectory for multi-turn paths. Under the total travel of 480 meters, the deviation of the starting end point was less than 2 meters, which was about 0.4% of the total travel.

Keywords

Personal Positioning, Inertial Navigation, Dead Reckoning, Map Matching, Particle Filtering

1. Introduction

In recent years, various applications based on people’s location information have been playing an important role in daily life, industrial production, transport and logistics, and emergency rescue. With the completion of the Global Navigation Satellite System (GNSS), all types of location services will enter a new era of comprehensive development. Although the satellite navigation system can achieve all-weather, all-day, high-precision global positioning and navigation, for the existence of signal blockage, such as indoor, underground, and other scenarios, there is a possibility of performance degradation and failure of satellite navigation, so it is significantly necessary to study the autonomous navigation and positioning technology in the case of satellite unavailability [1] . As a means of navigation and positioning with strong autonomous and anti-jamming properties, inertial navigation can be an effective complement to satellite navigation systems, thus solving the problem of locating and navigating people in the event of signal occlusion [2] .

There are two main technical approaches to current inertial-based means of personnel positioning systems: the zero-velocity updating (ZUPT) mechanism [3] and the pedestrian dead reckoning (PDR) [4] . The ZUPT system is based on the strap-down inertial solution and uses periodic footsteps to correct for velocity and position errors [5] , while the PDR system is simpler, using the person’s step length and heading to derive real-time position information [6] . However, for both systems, there is inevitably a cumulative error, i.e., the position error will dissipate over time and travel [7] [8] .

To solve the error dispersion problem in human inertial positioning, scholars around the world have proposed the use of particle filtering methods to introduce map information to suppress and correct position errors. Hideaki et al. [9] proposed a chest-worn IMU-based indoor localization system that uses person step and heading to derive position, uses map information to constrain the dispersion of position error, and constructs a particle back resampling method that effectively improves map matching performance with an average positioning error of less than 5.2 m for an 800 m trip. Koroglu et al. [10] proposed a multiple-hypothesis testing method for map matching of personnel inertial positioning, which significantly reduces the computational burden compared to particle filtering methods and enables the initialization of starting position and heading autonomously. To address the problems of limited utilization of map information, poor generality, and low accuracy of traditional particle filtering, Zhang et al. [11] constructed an adaptive particle filtering network, integrating the particle filtering algorithm into a neural network, and improving the utilization of map information through training and learning of the network to achieve accurate map matching and positioning. Luo et al. [12] evaluated the performance of three map-matching algorithms, implicit Markov model (HMM), particle filtering and geometric matching, under four spatial structures, and pointed out that the performance of the map-matching algorithm would be severely degraded under turnoffs and open space, and generate significant positioning errors. Xiong et al. [13] proposed a historical information-constrained personnel inertial localization error correction method, which used the inertial output to estimate the passable area in the environment and construct a 3D occupancy raster map, and the optimal estimation of personnel position and map by the particle filter-based Fast-SLAM method, with a localization error of no more than 4 m in a 1992 m walking experiment.

To address the problems of low map utilization and poor localization accuracy of forks in the current particle filtering map matching algorithm, this paper proposes an improved particle filtering-based map matching algorithm for inertial localization of people, with the following main contributions: 1) a multi-stage backtracking system for particle resampling is constructed, which can ensure the validity of resampled particles; 2) the traditional “non-passable constraint” of map matching is improved to “passable area guidance constraint”, which can significantly improve the localization accuracy of map matching. 3) the “impassability constraint” of traditional map matching is improved to “passable area guidance constraint”, which can significantly improve the positioning accuracy of map matching. Finally, the effectiveness and positioning performance of the algorithm was verified through outdoor positioning experiments, and the positioning error was less than 2 m in the 480 m walking experiment, which was about 0.41% of the total traveling distance.

2. Methods

2.1. Overall Structure

The solution in this paper chooses to wear inertial sensors (tri-axial accelerometer + tri-axial gyroscope) on the personnel’s chest, use the PDR system for personnel position projection, and then use the improved particle filtering algorithm to introduce map information to correct the PDR output personnel position, and finally achieve accurate and reliable personnel positioning, the overall architecture diagram is shown in Figure 1.

2.2. The Principle of Personnel Inertial Positioning

This paper will use the PDR system for inertial positioning projection of personnel, which mainly uses the step size and heading of the personnel movement to project the position information at the next moment, as shown in Equation (1).

$\{\begin{array}{l}{X}_k=X_{k-1}+L_k\cos \left({\theta }_k\right)\\ Y_k=Y_{k-1}+L_k\sin \left({\theta }_k\right)\end{array}$ (1)

where, $\left(X_k,Y_k\right)$ denotes the position coordinates at the moment k, $L_k$ denotes the step length, ${\theta }_k$ denotes the moving orientation.

Figure 1. The overall architecture of the algorithm.

The implementation of the PDR calculation of a person’s position requires three steps: stride detection, step-length estimation and heading solution.

1) Stride detection

In this paper, the peak method is used for stride detection, i.e., stride detection is performed by detecting the local maximum value of the tri-axial acceleration modulus, while a peak threshold is set to avoid false detection, and only when the local maximum value of the tri-axial acceleration modulus is greater than this threshold is a person determined to have moved a step, as shown in Equation (2).

$\text{step\_detector}=\{\begin{array}{cc}1& \text{if}\text{max}\left({\Vert Acc\Vert }_2\right)>\alpha \\ 0& \text{if}\text{max}\left({\Vert Acc\Vert }_2\right)\le \alpha \end{array}$ (2)

where, $\text{step\_detector}$ denotes the result of stride detection, $\text{max}\left({\Vert Acc\Vert }_2\right)$ denotes the local maximum value of the tri-axial acceleration modulus, $\alpha$ indicates step detection peak threshold.

2) Step-length estimation

In this paper, a Weinberg model with a non-linear step length model is used for the estimation of personnel movement step size, as shown in Equation (3).

$L_k=K\ast \sqrt[4]{\max \left({\Vert Acc\Vert }_2\right)-\min \left({\Vert Acc\Vert }_2\right)}$ (3)

where, $L_k$ denotes the estimated step length, $\text{max}\left({\Vert Acc\Vert }_2\right),\text{min}\left({\Vert Acc\Vert }_2\right)$ denote the local maximum value and minimum value of the tri-axial acceleration modulus, and K denotes the empirical constants.

3) Heading solution

The equivalent rotation vector algorithm is used to solve the attitude and then obtain the heading information of the personnel based on the attitude quaternion, as shown in Equation (4).

$\varphi \left(T\right)=\Delta {\theta }_1+\frac{1}{12}\Delta {\theta }_0\times \Delta {\theta }_1$ (4)

where, $\varphi$ denotes the rotation vector, $\Delta {\theta }_1$ denotes the angular increment at the current sampling moment, $\Delta {\theta }_0$ denotes the angular increment of the previous sampling moment.

After obtaining the equivalent rotation vector using Equation (4), the transformation yields the attitude change quaternion, as follows:

$q\left(T\right)=\cos \frac{\varphi }{2}+\frac{\varphi \left(T\right)}{\varphi }\sin \frac{\varphi }{2}$ (5)

The attitude change quaternion is then updated using the attitude change quaternion as follows.

$Q\left(T\right)=Q\left(T-1\right)\circ q\left(T\right)$ (6)

Converting the updated attitude quaternions to Euler angles gives information on the heading of the personnel.

2.3. Improved Particle Filtering Map Matching Algorithm

1) Algorithm flow

This paper is based on the SIR (Sampling Importance Resampling) particle filtering algorithm for improvement, the specific process of the algorithm is as follows:

a) Initialize the number of particles N, adopt Gaussian distribution ${\rm N}\left(\mu ,{\sigma }^2\right)$ to represent particle distribution, Set the variance and mean, the mean is the starting position of the person, the initial weights of all particles are 1/N.

b) Calculate recursively the position of each particle based on the step length and heading from the PDR.

c) Determine whether the particle is in the feasible region after one step of calculation, if it is, set the particle weight to 1, if not, set the particle weight to 0.

d) Calculate the effective particle coefficient according to Equation (7), if $N_{eff}<N_{th}$ , then perform the resampling.

$N_{eff}=\frac{{\displaystyle \underset{i=1}{\overset{N}{\sum }}{\omega }_i}}{N}$ (7)

where, $N_{th}$ denotes the threshold, this threshold can be taken as 0.8. ${\omega }_i$ is the particle weight.

e) Perform the resampling based on the multi-level backtracking strategy, and calculate the weights.

f) Cycle through steps (2) - (5) until positioning is complete.

2) Resampling strategy with multi-level backtracking

Particle degradation exists after multiple iterations of particle filtering, and an effective measure to address particle degradation is resampling. The resampling strategy directly determines the performance of the particle filtering. In this paper, by improving the traditional SIR resampling strategy, a multi-stage backtracking mechanism is introduced to guarantee the validity of the newly generated particles, thus improving the estimation accuracy of the particle filter. The specific process of the multistage backtracking resampling algorithm is as follows:

a) Set the mean value of the Gaussian distribution of particles as the position of the person after particle filtering at the current moment, set the variance of the Gaussian distribution and regenerate N particles to achieve particle initialization.

b) Check whether each particle is in the feasible region and set the weight of particles in the infeasible region to 0.

c) The particle with weight 1 is subjected to multi-stage backtesting, i.e. the particle position at the previous 1, 2 and m moments is backprojected according to the current moment position of the particle with weight 1. If the historical position of the particle is in the infeasible region during the back projection process, the weight of the particle is set to 0.

d) Calculate the effective particle coefficients using Equation (7), if $N_{eff}\ge N_{th}$ , end the resampling, if $N_{eff}<N_{th}$ , repeat steps (1)-(3). In addition, if the number of iterations is greater than $\beta$ , resampling also ends.

It is worth noting that in order to prevent excessive resampling times or getting stuck in a dead loop, a repeat execution threshold is introduced in step (4), which can typically be set to 10.

3. Experiment and Results

To verify the effectiveness and localization performance of the algorithms in this paper, multiple sets of walking experiments were conducted using the MTi-670 sensor from X-sens in the Netherlands. The inertial sensor is worn on the human body in the manner shown in Figure 2.

In the validation experiment, the tester moved 3 times along the specified trajectory, with the start and end points coinciding, for a total distance of 480 m, and the sensor sampling frequency was 100 Hz, and the 3-axis acceleration and 3-axis angular velocity data collected are shown in Figure 3. Due to the high sampling frequency of the data, the data curve is too dense and therefore only the inertia data for the period 200 s to 210 s is shown in Figure 3.

Figure 2. Inertial sensor wear schematic.

Figure 3. Inertia data for the movement.

The tri-axial acceleration data is used for step detection and step length estimation. The strong periodicity of the acceleration data is evident in Figure 3, where the acceleration data is used for step length estimation within each cycle. The estimated step length results can be seen in Figure 4.

Based on the collected three-axis gyroscope data, an equivalent rotation vector algorithm of “single subsample + previous subsample” is used to solve the attitude and obtain the personnel motion heading information, as shown in Figure 5.

With the help of digital image processing technology, manual annotation is used to mark out the feasible road areas in the map, and the annotated map is shown in Figure 6, in which the roads are marked as feasible areas.

The PDR algorithm and the algorithm in this paper were used to solve the personnel movement trajectory, and the deviation from the starting point and the endpoint was used as the index to measure the positioning accuracy, and the personnel movement trajectory obtained by the two algorithms is shown in Figure 7.

The starting and ending deviations of the motion trajectory are used as the positioning accuracy evaluation index, and the calculation results are shown in Table 1.

Figure 4. The estimated step length.

Figure 5. The calculated heading.

Figure 6. Marking of feasible areas in the map.

Figure 7. The trajectory of experiment.

Table 1. Deviations from the start and end points of the motion trajectory.

From Figure 7, the PDR algorithm gradually increases the positioning error with the increase of the movement distance, and the movement trajectory of 3 turns has serious deviation; the algorithm of this paper suppresses the dispersion of the position error by introducing the constraint of feasible region, and the movement trajectory of 3 turns has better repeatability. The above experimental results show that the algorithm can suppress the accumulation of personnel inertial positioning errors, improve the long-time positioning accuracy, have good repeatability for multi-turn trajectories, and achieve accurate and reliable personnel inertial positioning.

4. Conclusion

To address the problems of low map utilization and poor positioning accuracy of turn-offs in the current personnel inertial positioning algorithm, an improved particle filter-based map matching algorithm for personnel inertial positioning is proposed, and the performance of the algorithm is verified by conducting experiments on personnel inertial positioning. The experimental results show that, compared with the traditional PDR algorithm, the algorithm in this paper makes full use of the map constraint information and can effectively suppress the dispersion of the positioning error, with the positioning error less than 0.4% of the total travel. In future work, the integration of more information sources, such as vision and wireless positioning, will be considered to achieve accurate and reliable multi-source autonomous positioning.

Acknowledgements

This work was funded by the Science and Technology Project of State Grid Shanxi Electric Power Company, “Research and application of key technologies of elastic fusion security operation in digital region based on 5G and deep learning”, 5205C0220001.

Conflicts of Interest

The authors declare no conflicts of interest regarding the publication of this paper.

References

[1]	Chong, Y., Xu, X., Guo, N., Shu, L. and Zhang, Q. (2023) Adaptive Decentralized Cooperative Localization for Firefighters Based on UWB and Autonomous Navigation. Applied Sciences, 13, Article No. 5177. https://doi.org/10.3390/app13085177
[2]	Boguspayev, N., Akhmedov, D., Raskaliyev, A., Kim, A. and Sukhenko, A. (2023) A Comprehensive Review of GNSS/INS Integration Techniques for Land and Air Vehicle Applications. Applied Sciences, 13, Article No. 4819. https://doi.org/10.3390/app13084819
[3]	Zhang, L., Liu, Y. and Sun, J. (2021) A Hybrid Framework for Mitigating Heading Drift for a Wearable Pedestrian Navigation System through Adaptive Fusion of Inertial and Magnetic Measurements. Applied Sciences, 11, Article No. 1902. https://doi.org/10.3390/app11041902
[4]	Liu, Q., Ying, R., Dai, Z., Wang, Y., Qian, J. and Liu, P. (2023) Multi-Phase Fusion for Pedestrian Localization Using Mass-Market GNSS and MEMS Sensors. Sensors, 23, Article No. 3624. https://doi.org/10.3390/s23073624
[5]	Wang, Z., Xing, L., Xiong, Z., Ding, Y., Sun, Y. and Shi, C. (2022) An Improved Pedestrian Navigation Method Based on the Combination of Indoor Map Assistance and Adaptive Particle Filter. Remote Sensing, 14, Article No. 6282. https://doi.org/10.3390/rs14246282
[6]	Yamagishi, S. and Jing, L. (2022) Pedestrian Dead Reckoning with Low-Cost Foot-Mounted IMU Sensor. Micromachines, 13, Article No. 610. https://doi.org/10.3390/mi13040610
[7]	Xie, D., Jiang, J., Yan, P., Wu, J., Li, Y. and Yu, Z. (2023) A Novel Three-Dimensional Positioning Method for Foot-Mounted Pedestrian Navigation System Using Low-Cost Inertial Sensor. Electronics, 12, Article No. 845. https://doi.org/10.3390/electronics12040845
[8]	Wu, R., Lee, B.G., Pike, M., Zhu, L., Chai, X., Huang, L. and Wu, X. (2022) IOAM: A Novel Sensor Fusion-Based Wearable for Localization and Mapping. Remote Sensing, 14, Article No. 6081. https://doi.org/10.3390/rs14236081
[9]	Lu, C., Uchiyama, H., Thomas, D., Shimada, A. and Taniguchi, R.-I. (2019) Indoor Positioning System Based on Chest-Mounted IMU. Sensors, 19, Article No. 420. https://doi.org/10.3390/s19020420
[10]	Koroglu, M., Korkmaz, M., Yilmaz, A., et al. (2019) Multiple Hypothesis Testing Approach to Pedestrian INS with Map-Matching. 2019 International Conference on Indoor Positioning and Indoor Navigation (IPIN), Pisa, 30 September-3 October 2019, 1-8. https://doi.org/10.1109/IPIN.2019.8911787
[11]	Zhang, L., Cheng, M., Xiao, Z., et al. (2020) Adaptable Map Matching Using PF-Net for Pedestrian Indoor Localization. IEEE Communications Letters, 7, 1437-1440. https://doi.org/10.1109/LCOMM.2020.2984036
[12]	Luo, S., Gu, F., Xu, F. and Shang, J. (2020) Effect Evaluation of Spatial Characteristics on Map Matching-Based Indoor Positioning. Sensors, 20, Article No. 6698. https://doi.org/10.3390/s20226698
[13]	Ding, Y.M., Xiong, Z., Cao, Z.G., et al. (2021) Indoor Pedestrian 3D Inertial Positioning Error Correction Method Based on Historical Information Constraints. Journal of Chinese Inertial Technology, 6, 717-723.