2.2.3. Algorithm Flow

To sum up, the flow chart of pedestrian navigation and positioning method of IMAPF is shown in Figure 4.

**Figure 4.** Flow chart of the improved pedestrian navigation and positioning method based on IMAPF.

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 *x*ˆ*<sup>k</sup>* at the current moment is estimated by the updated adaptive number of particle sets and weights as

$$\pounds\_k = \sum\_{i=1}^{N} \widecheck{w}\_k^i \mathbf{x}\_k^i \tag{18}$$

The position *Pk* at the current moment is

$$P\_k = \sum\_{i=1}^{N} \tilde{w}\_k^i \left(\mathbf{x}\_k^i - \hat{\mathbf{x}}\_k\right) \left(\mathbf{x}\_k^i - \hat{\mathbf{x}}\_k\right)^T \tag{19}$$

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 *k*, *k* − 1, . . . . . . , 0 in turn.

Project the updated position onto the indoor map.

Step 5: *k* = *k* + 1, go to Step 2.

#### **3. Results**

*3.1. Verification of Simulation*

3.1.1. Conditions of Simulation

In order to verify the effectiveness of the improved pedestrian navigation and location method based on the indoor map assistance and particle filter, a series of simulation experiments were carried out. The simulation data is processed on the desktop computer, and the computer platform parameters are shown in Table 1.

**Table 1.** Computer platform parameters.


The simulation environment is based on the architectural plan of the fifth floor of no. 1 Building and no. 2 Building of the College of Automation Engineering, as shown in Figure 5. In Figure 5a, the red line indicates the corridor path. The four black dots (<sup>1</sup> <sup>2</sup> <sup>3</sup> <sup>4</sup> ) represent the reference point of the relative position.

The simulation movement trace is shown in Figure 6a. The blue point at the lower left corner is the starting point/end point. A complete closed loop path <sup>1</sup> →<sup>2</sup> →<sup>3</sup> →<sup>4</sup> →<sup>1</sup> has a distance of 207.72 m. The simulation moves two circles in a counterclockwise direction. The parameters are set as follows: The mean square error of the step noise is 0.1 m, and the mean square error of heading change noise is 1◦. The simulation data of pedestrian position and course change obtained are also saved. The four reference coordinates in the trace are shown in Figure 6b. Due to the process of entering the room, the total distance cannot be measured accurately, and the total distance exceeds 415.44 m.

Define the positioning error as

$$Err = \sqrt{\Delta x^2 + \Delta y^2} \tag{20}$$

In Equation (20), *Err* is the Euclidean Distance between the reference point and the measuring point. Δ*x* is the difference between the abscissa calculated by the proposed method and the abscissa calculated by the standard path; Δ*y* is the difference between the ordinate calculated by the proposed method and the ordinate calculated by the standard path.

**Figure 5.** Digital map on the fifth floor. (**a**) Architectural plan. (**b**) Available binary maps.

**Figure 6.** Simulation trace and coordinates of reference point. (**a**) Simulation trace. (**b**) Schematic diagram of the coordinates of the reference point.

#### 3.1.2. Analysis of Simulation Results

This section first analyzes the error comparison results between the IMAPF method and the PDR algorithm when the initial position and heading are known, then analyzes the positioning effect of the proposed method and PDR algorithm when the initial position and heading are unknown, and finally analyzes the navigation error and calculation efficiency when the initial position and heading are unknown and the adaptive particle number and fixed particle number.
