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

$$\begin{cases} \boldsymbol{\psi}\_k^i = \boldsymbol{\psi}\_{k-1}^i + \Delta \boldsymbol{\psi}\_k^i\\ \boldsymbol{\mu}\_k^i = \boldsymbol{\mu}\_{k-1}^i + l\_k^i \sin \boldsymbol{\psi}\_k^i\\ \boldsymbol{\upsilon}\_k^i = \boldsymbol{\upsilon}\_{k-1}^i + l\_k^i \cos \boldsymbol{\psi}\_k^i \end{cases} \tag{1}$$

In Equation (1), the system state quantity is the pedestrian position coordinate *ui <sup>k</sup>*, *<sup>v</sup><sup>i</sup> k* in the two-dimensional plane. The control quantity of the system is Δ*ψ<sup>i</sup> <sup>k</sup>* and *l i <sup>k</sup>*, where Δ*ψ<sup>i</sup> <sup>k</sup>* is the heading change, and *l i <sup>k</sup>* is the step length. Subscript *k* is step *k*-th step, and superscript *i* is the *i*-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.

**Figure 2.** Schematic diagram of the pedestrian motion trace update.

The control quantity equation of the particle filter based on PDR is:

$$\begin{cases} \prescript{f}{}{l\_k} = l\_k + \gamma\_{lk} \\ \Delta \hat{\psi}\_k = \Delta \psi\_k + \gamma\_{\psi k} \end{cases} \tag{2}$$

In Equation (2), *γlk* and *γψ<sup>k</sup>* 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:

$$l = \sqrt{(\mathbf{g}\_k - \mathbf{g}\_{k-1})^2 + (h\_k - h\_{k-1})^2} \tag{3}$$

In Equation (3), (*gk*, *hk*) and (*gk*−1, *hk*−1) 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.

**Figure 3.** Schematic diagram of the update of the particle position.

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 *Pi <sup>k</sup>* belongs to the accessible region *Pe* of the blue circle, it is regarded as "legal particle", and the particle weight *w<sup>i</sup> <sup>k</sup>* will not be changed. When a particle turns into an orange circle through state transition and enters the inaccessible area *Pm*<sup>1</sup> 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 *Pm*2. The particle weight value is set to 0, and the "illegal particle" is deleted. The equation is as follows:

$$w\_k^i = \begin{cases} 1 & P\_k^i \subset P\_k \\ 0 & P\_k^i \subset P\_{m1} \\ 0 & P\_k^i \subset P\_{m2} \end{cases} \tag{4}$$

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 *p*(*xk*|*y*1:*k*−1) 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 *w* 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 *n* particles *xi <sup>k</sup>*, *<sup>w</sup><sup>i</sup> k* (*i* = 1, 2, . . . , *n*), with

$$p(\mathbf{x}\_{0:k}|y\_{1:k}) \approx \sum\_{i=0}^{N} \tilde{w}\_k^i \delta \left(\mathbf{x}\_{0:k} - \mathbf{x}\_{0:k}^i\right) \tag{5}$$

$$\sum\_{i=0}^{N} \hat{w}\_k^i = 1 \tag{6}$$

In Equations (5) and (6), *N* represents the number of particles, *i* represents the number of each particle, *δ* represents the Dirac function, *x*0:*<sup>k</sup>* represents the historical system state in time interval 0 ∼ *k*, the observation quantity of the system is marked as *y*, representing the probability distribution, *y*1:*<sup>k</sup>* represents the historical observation in time interval 1 ∼ *k*, and *<sup>w</sup>*-*<sup>i</sup> <sup>k</sup>* represents the normalized importance weight of the *i*th particle at moment *k*.

At the initial moment, set *k* = 0, and randomly generate *N* sample particle groups *xi* <sup>0</sup>, *<sup>i</sup>* <sup>=</sup> 1, 2, . . . , *<sup>N</sup>* according to a priori probability *<sup>p</sup>*(*x*0). The weights of all particles are *wi* <sup>0</sup> <sup>=</sup> 1/*N*, and each particle sampled is recorded as *xi k*, 1/*N* .

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

$$p(\mathbf{x}\_0) \sim \mathcal{N}(\boldsymbol{\mu}, \sigma) \tag{7}$$

In Equation (7), *p*(*x*0) is the prior distribution of the initial position, *N*(*μ*, *σ*) 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:

$$p(\mathbf{x}\_0) \sim \mathcal{U}(a, b) \tag{8}$$

In Equation (8), *p*(*x*0) is the prior distribution of the initial position, *U*(*a*, *b*) is the uniform distribution, and *a* and *b* 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 *xk* at the current moment is estimated by the prior probability density of state *xk*−<sup>1</sup> at the previous moment. The prior distribution *p*(*x*0) of the initial state is known.

$$\begin{split} p(\mathbf{x}\_{k}|\mathbf{y}\_{1:k-1}) &= \int p(\mathbf{x}\_{k}, \mathbf{x}\_{k-1}|\mathbf{y}\_{1:k-1}) d\mathbf{x}\_{k-1} \\ &= \int p(\mathbf{x}\_{k}|\mathbf{x}\_{k-1}, \mathbf{y}\_{1:k-1}) p(\mathbf{x}\_{k-1}|\mathbf{y}\_{1:k-1}) d\mathbf{x}\_{k-1} \\ &= \int p(\mathbf{x}\_{k}|\mathbf{x}\_{k-1}) p(\mathbf{x}\_{k-1}|\mathbf{y}\_{1:k-1}) d\mathbf{x}\_{k-1} \end{split} \tag{9}$$

#### (3) Particle weight updating based on prior map information

Using the observation *yk* at the current moment, modify *p*(*xk*|*y*1:*k*−1) to obtain a posterior probability density *p*(*xk*|*y*1:*k*).

$$\begin{split} p(\mathbf{x}\_{k}|y\_{1:k}) &= \frac{p(\mathbf{y}\_{k}|\mathbf{x}\_{k},\mathbf{y}\_{1:k-1})p(\mathbf{x}\_{k}|y\_{1:k-1})}{p(\mathbf{y}\_{k}|\mathbf{y}\_{1:k-1})} \\ &= \frac{p(\mathbf{y}\_{k}|\mathbf{x}\_{k})p(\mathbf{x}\_{k}|y\_{1:k-1})}{p(\mathbf{y}\_{k}|\mathbf{y}\_{1:k-1})} \\ &= \frac{p(\mathbf{y}\_{k}|\mathbf{x}\_{k})p(\mathbf{x}\_{k}|y\_{1:k-1})}{\int p(\mathbf{y}\_{k}|\mathbf{x}\_{k})p(\mathbf{x}\_{k}|y\_{1:k-1})dx\_{k}} \end{split} \tag{10}$$

At moment *k* + 1, *yk*<sup>+</sup><sup>1</sup> 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

$$q(\mathbf{x}\_k^i \big| \mathbf{x}\_{k-1}^i, y\_k) = p(\mathbf{x}\_k^i \big| \mathbf{x}\_{k-1}^i) \tag{11}$$

The particles sampled according to this function are as follows:

$$\mathbf{x}\_k^i = q(\mathbf{x}\_k | \mathbf{x}\_{k-1}, \mathbf{y}\_{1:k}) \tag{12}$$

The normalized importance weight is

$$w\_k^i = w\_{k-1}^i \frac{p\left(y\_k \middle| \mathbf{x}\_k^i\right) p\left(\mathbf{x}\_k^i \middle| \mathbf{x}\_{k-1}^i\right)}{q\left(\mathbf{x}\_k^i \middle| \mathbf{x}\_{k-1}^i, y\_k\right)}\tag{13}$$

Substituting Equation (11) into Equation (13) has

$$w\_k^i = w\_{k-1}^i p\left(y\_k \middle| \mathbf{x}\_k^i\right) \tag{14}$$

In Equation (14), *p yk* # #*xi k* represents the detection result of the particle "not going through the wall" method.

The normalized weight calculation equation is

$$
\hat{w}\_k^i = \frac{w\_{k-1}^i}{\sum\_{j=1}^N w\_k^i} \tag{15}
$$
