Robust multi-constraint map matching algorithm for personal inertial positioning

ZHAO Hui,YAN Ruichao,ZHU Ning,SU Zhong,ZHANG Mengchao,LIU Gaowen

(1.Beijing Key Laboratory of High Dynamic Navigation Technology,Beijing Information Science and Technology University,Beijing 100192,China;2.Systems Engineering Research Institute,China State Shipbuilding Corporation Limited,Beijing 100094,China;3.School of Automation,Beijing Information Science and Technology University,Beijing 100192,China)

Abstract: To address the problem that traditional map-matching algorithms are prone to matching failure and accuracy degradation in bifurcated and semi-enclosed scenes,a robust multi-constraint map-matching algorithm for personnel inertial positioning is proposed.The algorithm constructs a geomagnetic/inertial fusion heading estimation system to calibrate the external geomagnetic interference in real time using a tri-axis gyroscope.The calibrated geomagnetic data are adopted to solve for heading,which could improve heading acquisition accuracy.Multiple constraints are imposed on the particle swarm trajectories within a fixed time window.This effectively increases the robustness of the algorithm.In addition,a heuristic multi-constraint resampling strategy is designed to enhance the adaptability of the particle swarm.Finally,several sets of indoor and outdoor long-distance walking experiments are conducted to evaluate the performance of the proposed algorithm.The results showed that,compared to particle filter algorithm for map backtracking constraints,the proposed algorithm is more robust,the localization accuracy could be improved by more than 2 times,and the localization error is less than 0.5% of total travel distance.

Key words: personal positioning;inertial measurement;geomagnetic information;particle filtering;map matching

The personal inertial positioning is an effective means to obtain personnel location information in a satellite denial environment.Current personal inertial positioning solutions can be divided into two categories:strapdown inertial navigation system (SINS) +zero-velocity updating (ZUPT) and pedestrian dead reckoning (PDR).In 2005,Foxlin described detailly the principle of the SINS+ZUPT scheme in Ref.[1].To introduce more error constraints,a variety of bipedal SINS+ZUPT schemes had been proposed.Wu[2]added an ultrasonic ranging module on the two-legged SINS+ZUPT scheme to construct a relative distance constraint,thus suppressing the error divergence.Li[3]proposed to wear two inertial sensors on one foot,and constructed constraints based on the relative distance between the two sensors.

Compared to SINS+ZUPT,the PDR scheme works more straightforwardly,which consists of three parts:gait detection,heading solution,and step-length estimation.In addition,the PDR scheme has no mandatory constraints on the mounting part of the inertial sensors.Considering the practicality,most of the current PDR solutions fix the sensors at the waist,feet,forehead,back,wrist,etc.To obtain accurate heading information,various methods have been proposed,such as the heuristic heading estimation algorithm[4]based on trajectory and building structure constraints.With the rise of artificial intelligence techniques,scholars worldwide have started to improve PDR schemes using neural networks.Wang[5]proposed a step-length online active learning method using long short term memory(LSTM) networks.Asraf[6]constructed a PDR network model using deep convolutional networks that can output heading and distance directly based on the angular velocity and acceleration information.What’s more,many scholars try to build a hybrid positioning scheme of SINS+ZUPT and PDR,such as in Ref.[7].

For SINS+ZUPT and PDR schemes,the cumulative errors are inevitable.If without the global position and attitude information,all optimization and correction strategies will only slow down the dispersion of cumulative errors and cannot eliminate them.To eliminate the cumulative errors,scholars have proposed to introduce map information to constrain the position and heading errors.Lu[8]used particle filter to introduce map constraint to correct the PDR trajectory and proposed a backtracking algorithm to check the validity of the newly generated particles.Wang[9]designed a tightly coupled ultra-wide band (UWB)+inertial navigation system (INS)+map scheme using an adaptive robust extended Kalman filter with a positioning accuracy of 0.27 m.Zhang[10]proposed an adaptive map-matching particle filter model using recurrent neural networks,which can improve the accuracy of traditional particle filter by 21%.Luo[11]proposed a PDR correction scheme using a dynamic time regularization method,which can eliminate the long-time accumulated errors.Gu[12]evaluated the performance of three map-matching algorithms,including hidden Markov model,particle filter,and geometric matching,and found that the algorithm performance will degrade under turnoffs and open spaces.In addition,if the original PDR position error is significant,it is difficult to improve the positioning accuracy even implementing map matching.

To achieve accurate and robust personnel positioning,a multi-level filtering scheme is proposed in this paper to fuse magnetic-angular rate-gravity (MARG)measurement and map information.First,based on the linear Kalman filter,the tri-axis geomagnetic measurement error can be corrected using the tri-axis angular rate information.Then the geomagnetic data on two horizontal axes are used to solve for the heading.Next,a heuristic random resampling particle filter method is adopted to fuse the map information to correct the errors of position,heading,and step length to achieve accurate personnel positioning under long-duration and long-time conditions.

The main contributions and innovations of this paper are as follows:

1) A novel heading estimation architecture for the pedestrian positioning is constructed,which can obtain accurate relative heading information using only geomagnetic data on two horizontal axes.In addition,the tri-axis angular rate information is used to compensate for the geomagnetic interference error.

2) A heuristic random resampling particle filter map matching algorithm is proposed,which can suppress the particle degradation phenomenon,and can feedback correct the step-length and heading error to achieve accurate and robust personal autonomous positioning.

1 Traditional PDR method

The traditional PDR method consists of three components: gait detection,heading solution,and step-length estimation.When the human body performs periodic movements (such as walking,running,and jumping),the period of human motion can be obtained through the tri-axis acceleration.The quaternion,equivalent rotation vector,Madgwick,and Mahony algorithm can be used to obtain the movement heading.In addition,the linear models,nonlinear models,and neural network models can be used to estimate the step length.Finally,the position can be calculated recursively from the last position,as shown in Eq.(1).

Where（x,y）is the position coordinate,Lkis the estimated step length at the momentk,ψkis the movement heading at the momentk.

2 Magnetic/inertial fused heading estimation

To obtain accurate and robust personnel heading information,a gyro-assisted modified geomagnetic Kalman filter algorithm is proposed in this paper.It uses only two-axis geomagnetic data to solve for the relative heading information.

2.1 Relative heading solution based on human rotation

According to the Ref.[13],the relationship between the magnetic field intensity in the geomagnetic coordinate system[BH0BZ]Tand the one in the body coordinate systemcan be described as follows:

By expanding and transforming the Eq.(2),we have:

Whereθ,,γ ψare the pitch,roll,yaw angles of the body,Iis the geomagnetic inclination.

As shown in Fig.1,when the human body is turned upright,the rotation angle can be obtained using the Eq.(4).At this point,it is equivalent to getting the relative heading of the human body.

Fig.1 The body coordinate system

Once the initial alignment is completed,the current heading information can be obtained based on the initial heading dataψ0and the relative heading information Δψk,as shown in Eq.(6).

2.2 Gyro-assisted correction of geomagnetic measurement errors

According to the previous section,the heading information can be calculated directly using the two-axis geomagnetic data.However,the geomagnetic sensor is susceptible to the influence of the external environment.And there are soft and hard magnetic interference errors,resulting in poor accuracy and robustness of the heading solution.As an inertial sensor with substantial autonomy,the gyroscope is not disturbed by the external environment.It can precisely perceive body rotation in a short period,so this paper uses the angular rate information of the gyroscope to construct a geomagnetic measurement error correction filter.

Assume that the tri-axis magnetometer output isB(t)，according to the Ref.[14],the relationship between tri-axis geomagnetic measurements and tri-axis angular rates can be described as:

Wherebis the bias of the measurement.

According to Eq.(7),the state equation can be established as:

Using the tri-axis geomagnetic output as the measurement,the measurement equation can be established as:

A first-order discretization of the state equation and the measurement equation yields:

The Kalman filter can be constructed based on the above state equation and measurement equation to compensate for the tri-axis geomagnetic measurement errors,as shown in Eq.(13).

WhereBˆ(k)is the compensated tri-axis geomagnetic measurement.

2.3 Accurate and robust heading information estimation

According to the above description,the specific flow of accurate and robust heading estimation using a tri-axis gyroscope and a tri-axis magnetometer is shown in Fig.2.

Fig.2 The flowchart of heading estimation algorithm

According to Fig.2,the measurement error of the tri-axis magnetometer can be corrected using the tri-axis gyroscope data.The state equation is constructed according to the relationship between the tri-axis geomagnetic vector change and the tri-axis angular rate,the measurement equation is constructed with the actual output of the tri-axis magnetometer.Then the geomagnetic measurement error can be estimated using the Kalman filter,thus realizing the online correction of geomagnetic sensor and the isolation of environmental interference.Finally,the corrected geomagnetic measurements of the two horizontal axes are used to calculate the relative heading of the personnel.

3 Robust multi-constraint map-assisted PDR

After obtaining reliable heading information,combined with the estimated step length,the PDR can be performed.However,the step-length models suffer from estimation errors.For long-time personnel positioning,the position error will accumulate quickly with time.Therefore,to realize accurate personnel positioning,additional absolute position information needs to be introduced to suppress the error dispersion.In this paper,the particle filter is used to introduce map information for the error compensation.

3.1 Step length estimation

A linear model is used to estimate the step length.

WhereL(k) is the step length at momentk.a(k)maxis the maximum value of the tri-axis acceleration mode at thek-th period.δL(k) is the step-length estimation bias.β1,β2are the pre-fitting model coefficients.

3.2 Map matching based on improved multi-constrained particle filtering

Based on the estimated step length and heading,the real-time PDR can be performed.To improve the accuracy of personnel positioning,a particle filter is used to introduce map constraint to achieve the precise position.There are several particle filter algorithms,but this paper adopts the sampling importance resampling(SIR) particle filter algorithm[15],as shown in Tab.1.

Tab.1 SIR particle filter algorithm

In the above description,x(k) denotes the state of the particle,that is,the position coordinates.w(k) is the particle weight.is the probability density function of particle distribution.

The basic idea of map-matching algorithm is to detect whether the position change of a particle penetrates a wall.If a particle penetrates the wall,its weight will set to zero.Then the estimation of personnel position is carried out based on the state and weight of the remaining particles.If the number of valid particles is lower than the set threshold,resampling is performed.

In this paper,based on the classical SIR particle filter,three main improvements are proposed.

1) Feedback correction of step and heading error

Suppose the position output of the original PDR isP(x(k),y(k)),the obtained heading isψ(k),the estimated step length isL(k),and the particle filter map corrected personnel position is,then the feedback of step length error and heading error are calculated according to the following equations.

Step length error feedback:

Heading error feedback:

The equation for PDR with the introduction of step length and heading error feedback correction is:

Whereβ3,β4are the feedback coefficients.

2) Multi-constrained particle weight update

The multiple constraints on the particle trajectory are as follows.1) The current moment particle and the previous moment particle link cannot go through the wall.2) The current moment particle and the previous moment estimated position link cannot go through the wall,which can prevent the emergence of multiple particle group branches.3) The particles cannot go beyond the map boundary.4) If all the particles fail,the particles are regenerated near the previous moment estimated position.The procedure is the same as the initialization of the particle swarm.The difference is the extra step to determine if a particle has passed through the wall.Though there is the case that all the particles go through the wall,they are regenerated at the original position and wait for the subsequent displacement action.This can suppress the influence of the step-counting error and the step-length estimation error.In addition,algorithm crashes can be avoided.

3) Heuristic multi-constraint resampling strategy

After several filtering cycles,the number of particles may degenerate,resulting in particle filter into local optimal solutions.To avoid such a phenomenon,resampling methods are used to generate new particles to overcome the particle degradation problem.Commonly used resampling methods include random resampling,importance resampling,residual resampling,etc.The new particles generated by different resampling methods are foreign,and the corresponding filter performance is significantly different.

The error sources that cause personnel position errors include three main categories: step-detect errors,heading errors,and step-length errors.These errors can lead to the occurrence of through-wall events,resulting in the reduction of the particles number,degradation,or even total failure.

By analyzing the particle motion law in the map matching process,a heuristic multi-constraint resampling strategy is proposed,and described as follows.

Step 1: After each filtering,check the number of valid particles.When the number of valid particles is less than the set threshold,enter Step 2.

Step 2: With the Gaussian distribution,the state of a new particle is randomly generated,and the particle state is shown in Eq.(18).

Step 3: Calculate the new particle weight.Detect if the connection between the old particle and the newly generated particle goes through the wall,if it does,the particle weight is 0,if not,the particle weight is 1.

Step 4: When the number of valid particles is greater than the threshold,stop generating new particles and end resampling.

Based on the above description,the pseudo-code of map-matching PDR algorithm based on multi-constraint particle filter is shown in Tab.2.

Tab.2 The map-matching PDR algorithm

The block diagram of the map-matching PDR algorithm based on multi-constraint particle filter is shown in Fig.3.

Fig.3 The block diagram of the proposed algorithm

The particle filter process is as follows.

Initialize the particle filter parameters,such as the number of particles,particle distribution radius,and the weight of particles.Set particle swarm distribution as Gaussian distribution.

Loop start:

1) update the particle position based on the step length and heading information.

2) detect whether the updated particle crosses the map range;if it does,the weight is set to zero.

3) detect whether the updated particle goes through the wall;if it does,the weight is set to zero.

4) detect whether the line between the current moment particle position and the previous moment estimated position through the wall;if it does,the weight is set to zero.

5) detect whether all particles are invalid;if it is,regenerate the particles around the estimated position at the previous moment.

6) calculate the personnel position based on the positions of all valid particles.

7) calculate the heading error correction and the step length error correction.

8) if the number of valid particles is less than the threshold,resampling is performed.Loop end.

4 Experiment and discussion

To verify the effectiveness and superiority of the algorithm in this paper,several experiments were carried out in indoor and outdoor environments using Ellipse-N nine-axis sensors produced by SBG,France.The map-matching algorithm proposed in this paper,the PDR algorithm,the particle filter algorithm for map backtracking constraints in the Ref.[8] (abbreviated as PDRBT),the map constrained accessible path algorithm in the Ref.[16] (abbreviated as MCAP),and the traditional particle filter algorithm (abbreviated as TPF)were used to track the position of testers.Among them,the PDR didn’t introduce map information and any constraints.The PDRBT imposed a through-wall constraint and a history trajectory constraint on particles.The TPF only adopted a through-wall constraint.The MCAP introduced map constraints by accessible paths.

The deviation between the starting and ending points was used to evaluate the positioning performance of the above five algorithms.The Ellipse-N sensor used in the experiments is shown in Fig.4.The indicator parameters of the sensors are shown in Tab.3.

Tab.3 SBG Ellipse-N nine-axis sensor index parameters

Fig.4 Sensor installation schematic

1) Outdoor positioning experiments

For the outdoor experiment,the experimental site was the Xiaoying Campus of Beijing Information Science and Technology University.The testers wore the sensors as shown in Fig.4 and moved according to the route shown in Fig.5.The total distance was about 990 meters.As shown in Fig.5(a),the red dot indicates the starting point,the endpoint overlaps with the starting point,and the yellow arrow indicates the motion direction.Since the starting and ending points overlap in the actual test,the deviation between the starting and ending points is used for the algorithm accuracy evaluation.Fig.5(b) shows the campus map obtained from Google Maps,where the blue line indicates the boundary on both sides of the road,and the particles cannot cross the road boundary.

Fig.5 Outdoor positioning experiment map information

In the outdoor experiment,the acceleration,angular velocity,and magnetic field strength information collected using the Ellipse-N 9-axis sensor are shown in Fig.6-Fig.8.

Fig.6 Tri-axis angular velocity measurement data

Fig.7 Tri-axis acceleration measurement data

Fig.8 Tri-axis geomagnetic measurement data

To suppress the interference of the external environment to the magnetometer,the gyro-assisted correction method is used to process the magnetometer outputs.The results are shown in Fig.9.

Fig.9 The geomagnetic correction results

As shown in Fig.9,the corrected tri-axis geomagnetic data have changed significantly.Due to the lack of geomagnetic reference measurement data,it is impossible to quantitatively evaluate the geomagnetic correction performance.However,the effectiveness of the geomagnetic correction algorithm will eventually be indirectly reflected by the positioning accuracy.

Then the corrected geomagnetic data is used to solve the heading,the result is shown in Fig.10.

Fig.10 Outdoor experimental heading solution results

Based on the collected sensor data and map information,the original PDR algorithm,the PDRBT algorithm,the TPF algorithm,the MCAP algorithm,and the algorithm proposed in this paper (represented by Ours) are used to solve and track the personnel location information.In particular,it should be noted that the step length estimation coefficientsβ1is 3,andβ2is 0.1.The number of particles is 300.The variance of the particle distribution is 5.The particle resampling threshold is 0.5.Based on the above parameters,the results are shown in Fig.11.

Fig.11 Outdoor positioning experimental results

The errors with the calculation of the starting and ending points are shown in Tab.4.

Tab.4 The starting and ending point deviations for outdoor positioning experiments

As can be seen from Fig.11,the algorithm proposed in this paper (green line in Fig.11) has the highest localization accuracy and the localization trajectory almost coincides with the actual movement trajectory.Tab.4 demonstrates that the location error of the proposed algorithm is 0.4% of total travel distance.Compared with the other algorithm,the algorithm in this paper improves the location accuracy by at least 3 times.The PDRBT algorithm (red line in Fig.11) also has a better localization accuracy,which is due to the simpler moving route,less route bifurcation,and lower requirements for the matching algorithm.The PDR algorithm (blue line in Fig.11) has an acceptable localization accuracy but the movement trajectory seriously deviates from the actual movement trajectory,which is caused by the lack of cumulative error suppression means.The TPF algorithm (purple line in Fig.11) has poor accuracy and there is an algorithm runaway during the movement,and all the particles become invalid,making the filtered position transform to the origin.The MCAP algorithm (black line in Fig.11) is improved compared with the traditional particle filter algorithm to avoid the situation that all particles are invalid,but it falls into a dead loop during the turning process,resulting in the position no longer changing.Obviously,the particles of MCAP algorithm all became invalid during the update process.

2) Indoor positioning experiments

The outdoor environment has more regular roads and fewer bifurcations,which requires less robustness of the particle filtering algorithm.To thoroughly verify the superiority of the proposed algorithm,indoor environment experiments were conducted.In the indoor walking experiment,the experimental site is a large shopping mall,and the testers wore the sensors as shown in Fig.4 and move according to the route shown in Fig.12.The total travel distance is about 780 meters.As shown in Fig.12,the red dot indicates the starting point of the motion,the endpoint coincides with the starting point,and the blue arrow indicates the direction of movement.Like the outdoor experiment,the deviation of the starting and ending points is used as the basis of accuracy evaluation.Fig.13 shows the map of the mall,where the blue line indicates the boundary of the route,and the particles cannot cross the boundary of the route.

Fig.12 Reference route for indoor positioning experiments

Fig.13 Indoor positioning experimental road border

In the indoor experiment,the acceleration,angular velocity,and magnetic field strength information collected using Ellipse-N 9-axis sensors are shown in Fig.14-Fig.16.

Fig.14 Tri-axis acceleration measurement data

Fig.15 Tri-axis angular velocity measurement data

Fig.16 Tri-axis geomagnetic measurement data

Like the above description,the proposed gyro-assisted correction method is used to process the magnetometer output.The results are shown in Fig.17.

Fig.17 The correction results for the indoor experiment

Next,the corrected geomagnetic data is used to solve the heading,and the result is shown in Fig.18.

Fig.18 Indoor experimental heading solution results

Based on the collected sensor data and map information,the original PDR algorithm,the PDRBT algorithm,the TPF algorithm,the MCAP algorithm,and the algorithm proposed in this paper (represented by Ours) are used to solve and track the personnel location information,and the solution results are shown in Fig.19.The errors with calculating the starting and ending points are shown in Tab.5.

Tab.5 The starting and ending point deviations of indoor positioning experiments

Fig.19 Indoor positioning experiment results

As shown in Fig.19,the algorithm proposed in this paper (green line in Fig.19) has the highest localization accuracy in the indoor complex environment,and the localization trajectory almost coincides with the actual motion trajectory.Tab.5 demonstrates that the location error of the proposed algorithm is 0.48% of total travel distance.However,the highest positional accuracy of the other algorithms is 10.43% of total travel distance.The algorithm in this paper have gained a huge improvement in localization accuracy at the order of magnitude level.This is because the algorithm in this paper adds additional constraints to the traditional particle filter matching algorithm,which enhances the robustness of the matching algorithm.Therefore,our method can adapt to more complex situations.The PDRBT algorithm (red line in Fig.19) and the TPF algorithm (purple line in Fig.19) shows a large map matching error in the middle part of the journey,resulting in a lower accuracy of the endpoint location.The MCAP algorithm (black line in Fig.19) showed a crash in the middle part of the journey,where all particles are invalid,resulting in the filtered position back to the origin.The conventional PDR algorithm (blue line in Fig.19) showed increasing localization error with increasing movement time,and the overall trajectory deviated severely from the actual movement trajectory.The map matching test in indoor environment verified the effectiveness and robustness of the proposed algorithm in this paper,which can better adapt to complex paths and avoid particle degradation and in-situ loops.

5 Conclusion

Personal inertial positioning is an essential means to achieve real-time position acquisition in a satellite denial environment.To suppress the rapid dispersion of cumulative inertial positioning errors,a particle filter algorithm is usually used to introduce map constraint.

However,current map matching algorithms have poor robustness,especially in complex routes and semi-closed scenarios.To solve the above problems,a robust multi-constraint personal inertial positioning map matching algorithm is proposed,which mainly includes two steps.Firstly,a magnetic-inertial fusion heading estimation scheme is established,which uses a tri-axis gyroscope to compensate the geomagnetic errors in real-time,and then adopts the compensated geomagnetic data to solve the heading.Secondly,to enhance the robustness of the particle filter map matching algorithm,a constraint on the particle swarm trajectory within a fixed time window is introduced.A heuristic multi-constraint resampling strategy is proposed to improve the adaptive capability of the particle swarm that can significantly correct the inertial cumulative error and achieve the reliable and accurate acquisition of the pedestrian position.At the end,the performance of the proposed algorithm is tested by several sets of indoor and outdoor long-distance walking tests.The results show that the positioning errors of the proposed algorithm can be less than 0.5% of total travel distance,which can fully meet the demand for autonomous pedestrian localization in the satellite denial environment.

Future work will focus on enhancing the performance of map-assisted pedestrian positioning algorithms by sensing special motion forms of pedestrian,such as riding elevators,taking stairs,turning at right angles,and matching them with notable landmarks in the map.In addition,the adaptive methods of the algorithm parameters also need to be investigated and more experiments on map-matching localization under complex paths need to be carried out in the future.