2.2.1. Theoretical Model of Algorithm
- (1)
Pedestrian navigation model based on dead reckoning
Figure 2 is the schematic diagram of the pedestrian motion trace update.
The state transition equation of particle filter of PDR can be obtained as
In Equation (1), the system state quantity is the pedestrian position coordinate in the two-dimensional plane. The control quantity of the system is and , where is the heading change, and is the step length. Subscript is step -th step, and superscript is the -th particle. The particle contains the possible two-dimensional position and heading at the current time. The position of the particle is corrected by constantly adjusting the particle weight and the two-dimensional position.
The control quantity equation of the particle filter based on PDR is:
In Equation (2), and represent step noise and heading change noise, respectively, which all obey the Gaussian distribution.
In this paper, the step length and heading change are calculated with the method of multiple constraints for indoor navigation (MCIN) [
30] based on multiple constraints, as the input of the IMAPF method proposed in this paper. This method consists of the following two parts: (a) A pedestrian navigation model based on ZUPT is constructed; (b) based on the feature that the difference between heading change of foot and heading change of waist is small in one step motion, a navigation correction model based on the consistency constraint of heading change of waist during human motion is constructed as a virtual observation. This method corrects the pedestrian navigation error in a period of time, but the error will increase when moving for a long time. As the motion state of adjacent steps does not change much in the motion process, the distance between the adjacent zero velocity intervals can be used as the step length control value through the MCIN method. In addition, since the waist IMU motion is relatively stable, the heading change obtained from the waist angular velocity integral is taken as the control value.
The step length is calculated as follows:
In Equation (3), and are the position coordinates of the current step and the previous step calculated by the MCIN method, respectively.
- (2)
Observation model based on particle “not going through the wall” method
In the navigation and positioning method based on the IMAPF, the “not going through the wall” method is to judge whether the current particle position is valid according to the position of the particle at the previous time after one step of movement.
To determine whether a particle is a “valid particle”, it is based on inaccessible areas in the map (such as patios, elevators, walls, etc.) or impossible paths in reality (such as going from one room to another without going through a door).
Figure 3 is a schematic diagram of particle motion. The particles representing the human at the last moment is marked as a white circle, and he is currently walking in the corridor.
According to the state equation, the particle coordinates at the current time can be obtained, including the following possible positions: Corridor, inside the wall, going through the wall into another room, etc. It is compared with the indoor accessible area (the accessible area is obtained through the architectural plan). If a particle position coordinate
belongs to the accessible region
of the blue circle, it is regarded as “legal particle”, and the particle weight
will not be changed. When a particle turns into an orange circle through state transition and enters the inaccessible area
such as the wall, it can be judged that this type of particle is an “illegal particle”, and then the weight value of this type of particle is set to 0. If a particle turns into a red circle and directly enters another room after the state transfer, there is no connectivity between the new particle and the particle at the previous moment, which belongs to an inaccessible area
. The particle weight value is set to 0, and the “illegal particle” is deleted. The equation is as follows:
The thickness of the wall is determined by the pixel length of the wall after the architectural plan is converted to the binary map. Generally, the physical length represented by one pixel is less than the thickness of the wall, that is, the wall is represented by at least several consecutive pixels. Even when the wall has only one pixel, it is possible to calculate that the particle is inside or through the wall, and then proceed to the next steps. Therefore, the accuracy of the method is independent of the thickness of the wall.
2.2.2. Algorithm Process Design
- (1)
Optimization of initial position and heading of particle set
The core idea of PF is that it is composed of a finite number of random samples (particles) with weight. Each particle represents the estimation of the current state. The integral operation of the posterior probability density distribution
is approximately expressed as the sum operation of the finite samples. The posterior probability distribution of the system is expressed by the density of the particle distribution. The value of the particle weight
represents the possibility of the state, which is used to represent the probability distribution of the state variable to approximate the true probability distribution of the system. Select
particles
, with
In Equations (5) and (6), represents the number of particles, represents the number of each particle, represents the Dirac function, represents the historical system state in time interval , the observation quantity of the system is marked as , representing the probability distribution, represents the historical observation in time interval , and represents the normalized importance weight of the th particle at moment .
At the initial moment, set , and randomly generate sample particle groups according to a priori probability . The weights of all particles are , and each particle sampled is recorded as .
The current research mainly focuses on the of navigation and positioning methods for indoor pedestrians when the initial positions and heading are known in a special working environment. The initial particles are added with Gaussian white noise, as shown in Equation (2), and the initial particles obey
In Equation (7), is the prior distribution of the initial position, is the Gaussian distribution, is the expectation, and is the mean square error.
However, in the actual environment, due to the special emergency of the work site and the inability to provide the absolute position information in a short time, when pedestrians must enter the work environment, this paper proposes a method based on global search to solve the pedestrian’s position and heading information. The initial particles are distributed in the entire indoor map in a uniform way, and the following equation holds:
In Equation (8), is the prior distribution of the initial position, is the uniform distribution, and and are the minimum and maximum values of the pixel coordinates in the picture, respectively.
- (2)
One-step prediction of particle states
According to the state equation, state
at the current moment is estimated by the prior probability density of state
at the previous moment. The prior distribution
of the initial state is known.
- (3)
Particle weight updating based on prior map information
Using the observation
at the current moment, modify
to obtain a posterior probability density
.
At moment , is updated, and the importance weight value of the whole state sequence needs to be recalculated, so the amount of calculation increases greatly over time. This problem is solved through Sequential Importance Sampling (SIS). A group of known random samples with weights is used to represent the posterior probability density, and the state estimation value is calculated based on the known random samples and weights.
The prior probability is selected as the importance density function, as follows
The particles sampled according to this function are as follows:
The normalized importance weight is
Substituting Equation (11) into Equation (13) has
In Equation (14), represents the detection result of the particle “not going through the wall” method.
The normalized weight calculation equation is
- (4)
Particle resampling based on adaptive particle number
In order to reduce the amount of calculation and avoid unnecessary resampling, judge whether resampling is required according to the effective particle number
. The equation for calculating the effective value of particles is
The smaller the number of effective particles is, the more serious the particle weight degradation is, and the there is more need to resample. Set the resampling threshold , when , particle set needs resampling.
Theoretically, the larger the number of particles, the more accurate the results of PF. However, as the number of particles increases, the calculation time increases exponentially, and the improvement of navigation and positioning accuracy is not obvious, especially for the navigation and positioning results under the condition of the known initial position and heading. For position under the condition of unknown initial position and heading, a large number of particles are required at the beginning. Using a large number of particles after finding the location of a pedestrian in the map will lead to computational inefficiency. Therefore, this paper proposes a method to adaptively adjust the particle number during resampling, which can greatly improve the computational efficiency of PF and ensure the high-precision navigation and positioning results of pedestrian navigation system. The calculation equation of the adaptive particle number is
In Equation (17), is the integral function, and is the sum of the main diagonal elements of the covariance matrix of the position coordinates of all particles at the current moment. and are empirical constants. is the proportional coefficient, and the value of ensures that the particle filter function can be achieved when is at the minimum value. In this paper, and are 1000 and 2000, respectively.
The updated particle set is recorded as .
2.2.3. Algorithm Flow
To sum up, the flow chart of pedestrian navigation and positioning method of IMAPF is shown in
Figure 4.
The algorithm flow is as follows:
Step 1: Initialization of position and heading of particle set: For the known initial position and heading, the initialization adopts Equation (7) to satisfy the Gaussian distribution. In the case of the unknown initial position and heading, the initialization adopts Equation (8) to meet the uniform distribution.
Step 2: Sequential importance sampling based on particle “not going through the wall” method: Update the particle weight according to the particle “not going through the wall” method, and then calculate the normalized weight value through the Equations (11)–(15).
Step 3: Particle resampling based on adaptive particle number: The particle number is adaptively calculated by Equations (16) and (17) and resample.
Step 4: State estimation and location update: The state
at the current moment is estimated by the updated adaptive number of particle sets and weights as
The position
at the current moment is
At moment k, when a particle is in an inaccessible area, the particle is resampled. Copy the navigation parameters (step length and heading at all times of 0~k) of the valid particle to the particle. The weighted average method is used to calculate the position of the pedestrian at the moment , , ……, 0 in turn.
Project the updated position onto the indoor map.
Step 5: , go to Step 2.