CN104061934A - Pedestrian indoor position tracking method based on inertial sensor - Google Patents

Abstract

The invention discloses a pedestrian indoor position tracking method based on an inertial sensor, relates to a pedestrian indoor position tracking method and particularly relates to a PDR (precision depth recorder) method and a PF fusion map information algorithm based on the inertial sensor. The pedestrian indoor position tracking method disclosed by the invention is used for solving the conditions such as great position estimation errors and even wrong estimation, and the like caused by long-time drift performance of the inertial sensor when only the PDR is used for tracking the pedestrian position. The method comprises the following steps of I. detecting steps and estimating step length according to an acceleration sensor; II. estimating a course angle according to the change of three-axle angular velocity in measured data of a gyroscope, correcting the course angle, carrying out course calculation according to the corrected course angle and the step length of the first step; and finally, estimating the position according to the step length and the course angle through the PDR method; and III. fusing map information with the PDR estimation by particle filtering to complete the pedestrian indoor position tracking method based on the inertial sensor. The pedestrian indoor position tracking method disclosed by the invention is applied to the technical field of indoor positioning.

Description

Pedestrian's indoor location tracking based on inertial sensor

Technical field

The present invention relates to a kind of pedestrian's indoor location tracking, the PDR method and the PF that are specifically related to based on inertial sensor merge cartographic information algorithm.

Background technology

In recent years, the development of microelectromechanical systems (Micro-ElectroMechanical Systems, MEMS), made inertial sensor be popularized in the application of intelligent mobile terminal.Pedestrian's indoor navigation system based on inertial sensor, without the low-cost advantage of laying outside plant, becomes research popular with it gradually.The ultimate principle of this system is to adopt pedestrian's dead reckoning (PDR, Pedestrian Dead-Reckoning), according to the measurement data of inertial sensor (as accelerometer, gyroscope), be mainly concerned with inspection step, estimate the technology such as step-length, estimated course angle, therefore this system is sometimes also referred to as SHSs (Step-and-Heading Systems).What adopt in early days is wearable device, by installation of sensors in shoes, the helmet, be placed in pocket, waist etc., in walking, motion feature can more be reflected in foot, so can better examine step based on foot's sensor, but need to additionally purchase Special Equipment and Portable belt not, be not suitable for common pedestrian's indoor navigation system.In the process of walking, acceleration transducer can be exported certain walking feature, by measurement data is carried out to peak value detection, zero passage detection, autocorrelation matching, spectrum analysis etc., wherein one or more combine to identify each step, and latter two causes real-time not strong because of length consuming time.For step-size estimation, the most rough is, and directly to set step-length be a constant, because pedestrian has an average step length under constant speed walking, but in reality, step-length is affected by a people's height, build, the factors such as frequency of changing step, and cannot be average without exception.The formula that researchers generally accept four root sides of a minimax acceleration difference calculates, but also someone proposes other as having linear relationship based on cadence and step-length, the sensor of dressing foot adopts Zero velocity Updating (ZUPT, Zero-velocity UPdaTe), based on step-size estimation methods such as pedestrian's heights.And in course estimation, the simplest method of estimation is that sensor is fixed on to pedestrian, make sensor coordinate system and terrestrial coordinate system (compass coordinate system, N-E coordinate system) three axles coincidences, the position angle that sensor records is like this pedestrian's course angle, often by by gravity, the component on acceleration transducer three axles and magnetometer (electronic compass) calculate and try to achieve, and generally, the placement of sensor is without fixed pose (being attitude arbitrariness), this is to carry out estimated course by other method, such as Principal Component Analysis Method (PCA, Primary Components Analyze) estimated course angle.

Particle filter (PF, Particle Filter) be the filter tracking algorithm rising in the Gauss of the state model of Kalman filtering, the ungratified situation of linear hypothesis condition, it,, by using Monte Carlo method sampling maximum a posteriori to estimate, is estimated thereby approach Bayes Optimum.Current various indoor positioning is as WLAN indoor positioning, UWB indoor location, all adopt particle filter to improve positioning precision and solve multiple target tracking problem based on vision indoor positioning, inertial sensor indoor positioning etc., and merge the data of different positioning systems with particle filter, improve positioning result thereby mutually assist.Often adopt particle filter that cartographic information is joined in location estimation, information is adjusted the parameter in particle filter according to the map, thereby not only improves precision, has also prevented that the generation of the special circumstances such as through walls from appearring in tracking target.

Summary of the invention

The present invention is will solve simple use PDR pedestrian position is followed the tracks of time, cause the greatly even situation such as wrong estimation of position estimation error because of drift that inertial sensor self has when long, and the tracking of the pedestrian's indoor location based on inertial sensor is provided.

Pedestrian's indoor location tracking based on inertial sensor is realized according to the following steps:

One, examine step and step-size estimation according to acceleration transducer;

Two, change and carry out course angle estimation according to three axis angular rates in gyroscope survey data, course angle is proofreaied and correct, then carry out reckoning according to course angle and first step step-length after proofreading and correct, finally pass through PDR method estimated position according to step-length, course angle:

${\mathrm{pos}}_k^{\mathrm{PDR}}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\end{array}\right]+{\mathrm{sLen}}_k\·\left[\begin{array}{c}\cos \left(h_k\right)\\ \sin \left(h_k\right)\end{array}\right]---\left(1\right)$

Wherein, represent the PDR estimated position of k step h _krepresent the course angle that k step is estimated, sLen _krepresent the step-length of k step;

Three, by particle filter, cartographic information and PDR estimated result are merged, completed the pedestrian's indoor location tracking based on inertial sensor.

Invention effect:

Pedestrian's location tracking method based on inertial sensor, by the inertial sensor module in smart mobile phone, pedestrian's dead reckoning position according to a preliminary estimate, then adopts particle filter algorithm to merge cartographic information, for user provides real-time position information.In pedestrian's dead reckoning realizes, a kind of inspection of the PZVT based on accekeration and time step algorithm is proposed, pass through step-size estimation, there is Excursion Characteristic when long for gyrostatic angle integration, utilizing three shaft angle degree to change mutually corrects, in experimental situation, test, position estimation error reduces relatively.Use particle filter combining cartographic information to carry out filtering to estimated position, improved to a certain extent the accuracy of position estimation.

Brief description of the drawings

Fig. 1 is the PDR theory diagram based on inertial sensor;

Fig. 2 is mobile phone sensor coordinate system, electronic compass coordinate system diagram;

Fig. 3 is based on inertial sensor pedestrian position tracking test environment schematic diagram;

Fig. 4 is Z axis accekeration and inspection step result;

Fig. 5 does not proofread and correct and proofreaies and correct rear course angle comparison diagram;

Fig. 6 uses PF and only uses the position of DR to follow the tracks of comparison diagram.

Embodiment

Embodiment one: the pedestrian's indoor location tracking based on inertial sensor of present embodiment is realized according to the following steps:

One, examine step and step-size estimation according to acceleration transducer;

Two, change and carry out course angle estimation according to three axis angular rates in gyroscope survey data, course angle is proofreaied and correct, then carry out reckoning according to course angle and first step step-length after proofreading and correct, finally pass through PDR method estimated position according to step-length, course angle:

${\mathrm{pos}}_k^{\mathrm{PDR}}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\end{array}\right]+{\mathrm{sLen}}_k\·\left[\begin{array}{c}\cos \left(h_k\right)\\ \sin \left(h_k\right)\end{array}\right]---\left(1\right)$

Wherein, represent the PDR estimated position of k step h _krepresent the course angle that k step is estimated, sLen _krepresent the step-length of k step;

Three, by particle filter, cartographic information and PDR estimated result are merged, completed the pedestrian's indoor location tracking based on inertial sensor.

Embodiment two: present embodiment is different from embodiment one: described step 1 detailed process is as follows:

Adopt the method in peak value-null value-valley-time interval to examine step, be that every row makes a move and comprises 1 peak acceleration, 2 null values, 1 minimum acceleration, and it being reasonable to meet the time interval, is that 2～4 steps are per second according to the speed of adult's constant speed walking, set interval down and be limited to 250 milliseconds, S ₀represent starting, S _i, i=1 ... 9 represent i step, and step-size estimation obtains by following formula

$\mathrm{sLen}=1.07\·\sqrt[3]{{\mathrm{acc}}_{\mathrm{Ave}}},{\mathrm{acc}}_{\mathrm{Ave}}=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\left|{\mathrm{acc}}_i\right|}{N}---\left(2\right)$

Wherein sLen is the step-length of estimating, acc _i, acc _averepresent respectively accekeration, average acceleration value in each step, N represents the data amount check collecting in every step.

Other step and parameter are identical with embodiment one.

Embodiment three: present embodiment is different from embodiment one or two: in described step 2, course angle method of estimation is:

First three axis angular rates around x, y, z are carried out to time integral, obtain respectively the angle of pitch, roll angle, position angle, be denoted as Pitch, Roll, Azimuth;

Adopt (3) to carry out first step correction to course angle:

heading＝c ₁·Pitch+c ₂·Roll+c ₃·Azimuth

(3) wherein, angle of pitch Pitch represents round the rotation amount of x axle, and roll angle Roll represents that, round the rotation amount of y axle, position angle Azimuth represents round the rotation amount of z tri-axles, wherein c ₁, c ₂, c ₃for corresponding weighting coefficient;

Course angle after the first step is proofreaied and correct is carried out second step correction, carries out successively smoothly, specifically calculates by following formula:

${\mathrm{heading}}_i=\mathrm{mean}\left(\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{heading}}_k\right)---\left(4\right)$

Mean () represents to average.

Other step and parameter are identical with embodiment one or two.

Embodiment four: present embodiment is different from one of embodiment one to three: described step 3 pass through particle filter by as follows to cartographic information and PDR estimated result fusion method:

Particle filter is a kind of Bayes's filtering method that adopts monte carlo method to approach posterior probability, does not require that kinematic system is linearity or Gauss;

The state equation of the particle filter one, adopting is:

$s_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& (l_k+n_k^l)\·\cos (\·)\\ 0& 1& (l_k+n_k^l)\·\sin (\·)\end{array}\right]\·\begin{array}{}\end{array}\begin{array}{}\end{array}(\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ h_k\end{array}\right]+\begin{array}{}\end{array}\left[\begin{array}{c}0\\ 0\\ n_k^h\end{array}\right])---\left(5\right)$

Wherein, x _k, y _kposition coordinates while representing k step, x _k-1, y _k-1represent the position coordinates of k-1 step, h _kthe course angle estimating, for step-size estimation error, for course angle estimation error, l _krepresent?, sin (), cos () represents to ask respectively sine value, the cosine value of corresponding angle;

Two, all particle value of the being weighted w to k step _icalculate:

$w_i=\left\{\begin{array}{c}\exp (-\frac{{\left|\right|{\mathrm{pos}}_k^{\mathrm{PDR}}-{\mathrm{pos}}_k^i\left|\right|}^2}{2{\mathrm{\σ}}^2}),\mathrm{condition}\_1\\ 0,\mathrm{condition}\_2\end{array}\right.---\left(6\right)$

Wherein represent the PDR estimated position of k step the position that represents i particle of k step, σ represents the standard deviation of PDR location estimation, in the time that certain particle generating in k step is not within the scope of active position, w _iget 0 value, otherwise calculate weights according to condition_1;

Three, by particle filter, cartographic information and PDR estimated result are merged:

At all particle weighted value w that obtain k step _iafter, according to w _icalculating has been used the estimated position of the k step of particle filter:

${\mathrm{pos}}_k^{\mathrm{PF}}=\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{\tilde{w}}_i\·{\mathrm{pos}}_k^i---\left(7\right)$

Wherein, Np represents the particle number in k step, represent the estimated position to i particle in k step calculating according to formula (5), is normalization weighted value, calculates by following formula:

${\tilde{w}}_i=\frac{w_i}{\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{w}_i}---\left(8\right)$

Particle filter is carried out in the position constantly PDR being estimated, and obtains new estimated position.

The state estimation of dynamic system is generally described by two equations, is respectively state equation: s _k=F (s _k-1, n _k-1) (9)

Observation equation: m _k=G (s _k, v _k) (10)

Wherein F (), G () is system state s _k-1, s _kfunction, s _kfor k moment state variable, m _kfor s _kobserved reading, system noise n _kwith observation noise v _kfor known separate of probability density, and with independently stochastic variable of system state;

Other step and parameter are identical with one of embodiment one to three.

Below in conjunction with embodiment is elaborated, the pedestrian's location tracking method based on inertial sensor is realized by following step:

One, examine step and step-size estimation according to acceleration transducer:

In conjunction with Fig. 2, Fig. 3, embodiment one is described in detail.The planimetric map signal of the research park 2A of Tu2Shi Harbin Institute of Technology 12 floor, follows the tracks of experiment (dark fill area in figure) in the corridor of this floor based on inertial sensor pedestrian position, and wide 3 meters of left and right, corridor, grow up approximately 90 meters.What in Fig. 3, show is in experiment test, the attitude of tester's handheld mobile phone, and mobile phone inner sensor coordinate system (Sensor coordinate system), navigation coordinate system (E-N coordinate system).

People in the process of walking, whether the acceleration change that the different parts of health can be different, but all regular, so identify and walk, or the action such as static by therefrom information extraction.Adopt the method in peak value-null value-valley-time interval (PZVT) to examine step, be that every row makes a move and comprises 1 peak acceleration, 2 null values, 1 minimum acceleration (as shown in Figure 4), and it is reasonable to meet the time interval, be that 2-4 step is per second according to the speed of adult's constant speed walking, set interval down and be limited to 250 milliseconds.Effective step that the summit box indicating of waveform detects, S ₀represent starting, S _i, i=1 ... 9 represent i step.

Actual measurement PZVT inspection step algorithm is known, and the inspection step arithmetic accuracy that present embodiment adopts is substantially 100%, and flase drop, situation undetected, many inspections seldom occur.

Step-length is not only relevant with people's height, and also relevant with the factor such as cadence in walking, same person each step in walking is all variant, so step-size estimation is a very challenging problem.This patent adopts formula

$\mathrm{sLen}=k\·\sqrt[3]{{\mathrm{acc}}_{\mathrm{Ave}}},{\mathrm{acc}}_{\mathrm{Ave}}=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\left|{\mathrm{acc}}_i\right|}{N}---\left(1\right)$

Wherein sLen is the step-length of estimating, acc _i, acc _averepresent respectively accekeration, average acceleration value in each step, k is coefficient corresponding to each method, value 1.07 of the present invention, and N represents the data amount check collecting in a step.

Two, carry out course angle estimation according to gyroscope survey data.Consider that in actual walking, mobile phone is not always all in above-mentioned basic poses, in the process of walking about, can be along with pedestrian's step alternately has slight swinging, and the angle disturbance causing due to center of gravity dipping and heaving etc., so adopt (3) to carry out first step correction to it:

heading＝c ₁·Pitch+c ₂·Roll+c ₃·Azimuth (2)

Wherein Pitch, Roll, Azimuth shown in corresponding diagram 2, is wherein respectively the angle of pitch, roll angle, position angle round the rotation amount of x, y, z three axles respectively, is denoted as Pitch, Roll, Azimuth, c ₁, c ₂, c ₃for corresponding weighting coefficient, need in actual scene, test and obtain, in this experiment, assignment is c ₁=1, c ₂=0.3, c ₃=0.1.On this basis the angle obtaining is carried out to second step correction, carry out successively level and smooth, be mainly in order to prevent that thereby the course angle precision in a certain step from appearring affecting in jagged value, but smoothly only limit to straight line moving or more stable process, as the horizontal segment of the curve in Fig. 5, specifically calculate by following formula

${\mathrm{heading}}_i=\mathrm{mean}\left(\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{heading}}_k\right)---\left(4\right)$

Wherein, mean () represents to average.

Three, by particle filter, cartographic information and PDR estimated result are merged.Simplify motion model with respect to Kalman filtering (KF, Kalman Filtering), thereby try to achieve optimum solution, particle filter is not simplified model, but the suboptimal solution of direct calculation of complex motion model, thereby approach gradually optimum solution.Particle filter is a kind of Bayes's filtering method that adopts monte carlo method to approach posterior probability, and not needing to set motion model is the starting condition such as linear model.

The state estimation of dynamic system is generally described by two equations, is respectively

State equation: s _k=F (s _k-1, n _k-1) (9)

Observation equation: m _k=G (s _k, v _k) (10)

Wherein F (), G () is system state s _k-1, s _kfunction, s _kfor k moment state variable, m _kfor s _kobserved reading, system noise n _kwith observation noise v _kfor known separate of probability density, and with independently stochastic variable of system state.

The state equation of the particle filter adopting in present embodiment is

$s_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& (l_k+n_k^l)\·\cos (\·)\\ 0& 1& (l_k+n_k^l)\·\sin (\·)\end{array}\right]\·\begin{array}{}\end{array}\begin{array}{}\end{array}(\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ h_k\end{array}\right]+\begin{array}{}\end{array}\left[\begin{array}{c}0\\ 0\\ n_k^h\end{array}\right])---\left(5\right)$

Wherein, x _k, y _kposition coordinates while representing k step, h _kthe course angle estimating according to embodiment two, for step-size estimation error, for course angle estimation error, sin (), cos () represents to ask respectively sine value, the cosine value of corresponding angle.

$w_i=\left\{\begin{array}{c}\exp (-\frac{{\left|\right|{\mathrm{pos}}_k^{\mathrm{PDR}}-{\mathrm{pos}}_k^i\left|\right|}^2}{2{\mathrm{\σ}}^2}),\mathrm{condition}\_1\\ 0,\mathrm{condition}\_2\end{array}\right.---\left(6\right)$

Wherein represent the PDR estimated position of k step the position that represents i particle of k step, σ represents the standard deviation of PDR location estimation.In the time that certain particle generating in k step is not within the scope of active position (such as the situation such as through walls), w _iget 0 value, otherwise calculate weights according to condition_1.

Specific implementation process is: a first given initial position (0, 0), whether then detect is an effectively step, if, estimate that this is step by step long, course angle, according to step-length, course angle is estimated rough position by PDR, in conjunction with random 100 particles that produce (all needing to regenerate in each step), calculate the position of particle, then calculate the weights of each particle according to the condition of (8), all particle position weighted sums are obtained to the filtered position of PF, then repeat above process and carry out the next position estimation, each step of double counting is put like this, realize pedestrian's position has been followed the tracks of.

Four, the last NEXUS5 smart mobile phone image data of passing through in the experimental situation shown in Fig. 3 (is placed attitude according to the mobile phone in Fig. 2, tester's handheld mobile phone is the capable record data that take a round in experiment corridor), the data that collect have accekeration, magnitude of angular velocity and sampling time, then in MATLAB2012b, carry out simulation calculation, finally obtain curve as shown in Figure 6 according to proposed algorithm.Can see, be compared to and only use PDR estimated position through pedestrian's location estimation method of PZVT inspection step, course angle correction and PF fusion cartographic information, improve estimation degree of accuracy.

First a given initial position (0, 0), whether then detect is an effectively step, if, estimate that this is step by step long, course angle, according to step-length, course angle is estimated rough position by PDR, in conjunction with random 100 particles that produce (all needing to regenerate in each step), calculate the position of particle, then the condition of basis is calculated the weights of each particle, all particle position weighted sums are obtained to the filtered position of PF, then repeat above process and carry out the next position estimation, each step of double counting is put like this, realize pedestrian's position has been followed the tracks of.

Claims (4)

1. the pedestrian's indoor location tracking based on inertial sensor, is characterized in that it realizes according to the following steps:

One, examine step and step-size estimation according to acceleration transducer;

Two, change and carry out course angle estimation according to three axis angular rates in gyroscope survey data, course angle is proofreaied and correct, then carry out reckoning according to course angle and first step step-length after proofreading and correct, finally pass through PDR method estimated position according to step-length, course angle:

${\mathrm{pos}}_k^{\mathrm{PDR}}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\end{array}\right]+{\mathrm{sLen}}_k\·\left[\begin{array}{c}\cos \left(h_k\right)\\ \sin \left(h_k\right)\end{array}\right]---\left(1\right)$

Wherein, represent the PDR estimated position of k step h _krepresent the course angle that k step is estimated, sLen _krepresent the step-length of k step;

Three, by particle filter, cartographic information and PDR estimated result are merged, completed the pedestrian's indoor location tracking based on inertial sensor.

2. according to the pedestrian's location tracking method based on inertial sensor described in claim 1, it is characterized in that described step 1 detailed process is as follows:

Adopt the method in peak value-null value-valley-time interval to examine step, be that every row makes a move and comprises 1 peak acceleration, 2 null values, 1 minimum acceleration, and it being reasonable to meet the time interval, is that 2～4 steps are per second according to the speed of adult's constant speed walking, set interval down and be limited to 250 milliseconds, S ₀represent starting, S _i, i=1 ... 9 represent i step, and step-size estimation obtains by following formula

$\mathrm{sLen}=1.07\·\sqrt[3]{{\mathrm{acc}}_{\mathrm{Ave}}},{\mathrm{acc}}_{\mathrm{Ave}}=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\left|{\mathrm{acc}}_i\right|}{N}---\left(2\right)$

Wherein sLen is the step-length of estimating, acc _i, acc _averepresent respectively accekeration, average acceleration value in each step, N represents the data amount check collecting in every step.

3. according to the pedestrian's location tracking method based on inertial sensor described in claim 2, it is characterized in that in described step 2, course angle method of estimation is:

First three axis angular rates around x, y, z are carried out to time integral, obtain respectively the angle of pitch, roll angle, position angle, be denoted as Pitch, Roll, Azimuth;

Adopt (3) to carry out first step correction to course angle:

heading＝c ₁·Pitch+c ₂·Roll+c ₃·Azimuth (3)

Wherein, angle of pitch Pitch represents round the rotation amount of x axle, and roll angle Roll represents that, round the rotation amount of y axle, position angle Azimuth represents round the rotation amount of z tri-axles, wherein c ₁, c ₂, c ₃for corresponding weighting coefficient;

Course angle after the first step is proofreaied and correct is carried out second step correction, carries out successively smoothly, specifically calculates by following formula:

${\mathrm{heading}}_i=\mathrm{mean}\left(\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{heading}}_k\right)---\left(4\right)$

Mean () represents to average.

4. according to the pedestrian's location tracking method based on inertial sensor described in claim 3, what it is characterized in that described step 3 passes through particle filter by as follows to cartographic information and PDR estimated result fusion method:

Particle filter is a kind of Bayes's filtering method that adopts monte carlo method to approach posterior probability, does not require that kinematic system is linearity or Gauss;

The state equation of the particle filter one, adopting is:

$s_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& (l_k+n_k^l)\·\cos (\·)\\ 0& 1& (l_k+n_k^l)\·\sin (\·)\end{array}\right]\·(\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ h_k\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ n_k^h\end{array}\right])---\left(5\right)$

Wherein, x _k, y _kposition coordinates while representing k step, x _k-1, y _k-1represent the position coordinates of k-1 step, h _kthe course angle estimating, for step-size estimation error, for course angle estimation error, l _krepresent?, sin (), cos () represents to ask respectively sine value, the cosine value of corresponding angle;

Two, all particle value of the being weighted w to k step _icalculate:

$w_i=\left\{\begin{array}{c}\exp (-\frac{{\left|\right|{\mathrm{pos}}_k^{\mathrm{PDR}}-{\mathrm{pos}}_k^i\left|\right|}^2}{2{\mathrm{\σ}}^2}),\mathrm{condition}\_1\\ 0,\mathrm{condition}\_2\end{array}\right.---\left(6\right)$

Wherein represent the PDR estimated position of k step the position that represents i particle of k step, σ represents the standard deviation of PDR location estimation, in the time that certain particle generating in k step is not within the scope of active position, w _iget 0 value, otherwise calculate weights according to condition_1;

Three, by particle filter, cartographic information and PDR estimated result are merged:

At all particle weighted value w that obtain k step _iafter, according to w _icalculating has been used the estimated position of the k step of particle filter:

${\mathrm{pos}}_k^{\mathrm{PF}}=\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{\tilde{w}}_i\·{\mathrm{pos}}_k^i---\left(7\right)$

Wherein, Np represents the particle number in k step, represent the estimated position to i particle in k step calculating according to formula (5), is normalization weighted value, calculates by following formula:

${\tilde{w}}_i=\frac{w_i}{\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{w}_i}---\left(8\right)$

Particle filter is carried out in the position constantly PDR being estimated, and obtains new estimated position.