## 3 System Model
### AO-PF Fusion Algorithm
#### 3.6.2 AO-PF Utilized in WiFi/PDR Fusion  
For the AO-PF fusion method, initialization is first performed, and then, the following observation equation is calculated. (1) Equation of State  
The following state equation is obtained based on the PDR heading angle and position update coordinates. \[X_{k}= \begin{bmatrix}H_{k}\\ x_{k}\\ y_{k}\end{bmatrix} = \begin{bmatrix}H_{k-1}\\ x_{k-1}\\ y_{k-1}\end{bmatrix} + \begin{bmatrix}\Delta H_{k-1}\\ SL_{k}\text{cos}(H_{k})\\ SL_{k}\text{sin}(H_{k})\end{bmatrix} + \begin{bmatrix}W_{H,k-1}\\ W_{x,k-1}\\ W_{y,k-1}\end{bmatrix}  \tag{25}\]  
where \(H_{k}\) denotes the heading at \(k\) epoch, \(x_{k}\) _and_\(y_{k}\) denote the coordinates of the PDR part at \(k\) epoch, and \(W\) is the normally distributed noise. (2) Observation Equation  
\[Z_{wifi,k}= \begin{bmatrix}x_{wifi-PSO}\\ y_{wifi-PSO}\end{bmatrix} + \begin{bmatrix}v_{x}\\ v_{y}\end{bmatrix}  \tag{26}\]  
where \(Z_{k}\) represents the observed position at \(k\) epoch, \(\left[x_{wifi-PSO}\quad y_{wifi-PSO}\right]\) denotes the position coordinates obtained by the \(PSO\) model for the WiFi portion, including both the \(x\)-coordinate and the \(y\)-coordinate, and \(\left[v_{x}\quad v_{y}\right]\) is typically modeled as Gaussian noise representing the noise component affecting the \(x\)-coordinate and \(y\)-coordinate measurement. (3) AO Algorithm Generates New Particles  
The AO algorithm reduces the problem of particle impoverishment by adding new particles. The way AO generates particles can be referred to in the previous Formulas (19)-(24), which are also divided into four steps (initialization, comprehensive elimination, local elimination, and post-consolidation). In order to prevent particles from escaping the boundary, a new step 5 is added, which is the boundary check step. The formula of the boundary check step is as follows:\[z_{j}=\left\{ \begin{aligned} z_{j},&\quad\quad if\;lb\leq z _{j}\leq ub\\ best_{j},&\quad\quad if\;z_{j}<lb\;or\;\;z_{j}>ub\end{aligned} \right. \tag{27}\]  
where \(z_{j}\) denotes the variable that needs to have its bounds checked, \(lb\) and \(ub\) represent lower boundary and upper boundary, respectively, and \(best_{j}\) is the value of the corresponding dimension j in the current optimal solution. In this AO algorithm, fitness will use the following formula:  
\[fitness=\left\| \begin{bmatrix}X_{AO}\\ Y_{AO}\end{bmatrix} -Z_{wifi,k}\right\| \tag{28}\]  
where \(\left[X_{AO}\quad Y_{AO}\right]\) is the current solution obtained by the \(AO\) algorithm, representing the position vector of a particle, \(Z_{wifi,k}\) is the WiFi observation at \(k\) epoch, and \(\left\|\cdot\right\|\) denotes the Euclidean distance. After the particles have gone through these five stages, their positions are updated. These updated particles are then merged with the particles generated by the PF, forming a new particle set. Weight Update  
The following formula describes the weight calculation process of each particle in the particle filter. Specifically, it is based on the likelihood estimation of the Gaussian distribution. \[\omega_{k}^{i}=\frac{1}{\sqrt{|R|}.\sqrt{2\pi}}e^{-\left[Z_{k}-h(x_{k}^{i}) \right]^{T}R^{-1}\left[Z_{k}-h(x_{k}^{i})\right]} \tag{29}\]  
where \(R= \begin{bmatrix}0.3&0\\ 0&0.3\end{bmatrix} \) denotes the measurement noise covariance matrix. Then, the weights are normalized to obtain the following formula. \[\widetilde{\omega}_{k}^{i}=\frac{\omega_{k}^{i}}{\sum_{i=1}^{N}\omega_{k}^{i}} \tag{30}\]  
(5) Resampling  
Then, the particles are resampled based on the accumulated weights. \[x_{i}^{k+1}=x_{j}^{k}\;\;\left(j=min\left\{j|c_{j}\geq u_{i}\right\}\right) \tag{31}\]  
where \(c_{j}=\sum_{n=1}^{j}\widetilde{\omega}_{n}^{k}\) denotes the cumulative weight, and \(u_{i}\) represents a random number between 0 and 1. Finally, the final positioning is performed by calculating the average value of the resampled particles through state estimation.