## 3 FastSLAM-PSO  
FastSLAM relies on particle filtering to estimate the pose of the robot and employs the EKF to update landmark estimates. However, in complex and high-dimensional environments, the particle distribution may drift away from the optimal region over time, leading to a decline in estimation accuracy. By integrating the PSO algorithm into the particle filtering process of FastSLAM, particles are more effectively concentrated in regions of high posterior probability, thereby improving overall estimation accuracy. PSO adjusts the velocity and position of particles in such a way that they follow the global best solution found by the swarm while maintaining their individual exploration space, ensuring sufficient diversity. This mechanism is particularly suitable for the nonlinear pose estimation problem in FastSLAM, and PSO can assist the particle swarm in quickly converging to a reasonable pose solution, even in the presence of complex environments or high levels of noise. Within the particle filtering framework, each particle is characterized by its positional coordinates \(\{x,y\}\) and its orientation \(\theta\). In our FastSLAM-PSO, the population size parameter \(N\) of PSO is controlled by the number of particles in the FastSLAM algorithm. That is, after each time step, we optimize positional coordinates \(\{x,y\}\) and orientation \(\theta\) of particles based on the current particle distribution, which allows the particles themselves to move directly rather than indirectly, which enhances the simplicity of the algorithm. Our objective function is designed based on the measurement probability value associated with a given particle state. A higher measurement probability indicates a greater likelihood of the presence of a particle at that location, which is conducive to subsequent particle filtering operations. The PSO objective function is calculated as follows:  
\[maxf(z)=\prod_{k=1}^{K}P(z_{t,k}|s,\Theta_{k}) \tag{11}\]  
where \(P\) (\(z_{t,k}\,|\,s,\Theta_{k}\)) is the probability of observing the \(k\)-th landmark given the pose of the robot and the position of the \(k\)-th landmark. The expression of PSO within FastSLAM can be combined with the state and importance weight of the particles to update the velocity and position of the particles. For the \(m\)-th particle, the velocity update formula is:  
\[v_{m}^{(t+1)}=w\cdot v_{m}^{(t)}+c_{1}\cdot rand()\cdot(pbest_{m}-x_{m}^{(t)} )+c_{2}\cdot rand()\cdot(gbest-x_{m}^{(t)}) \tag{12}\]where \(v_{m}^{(t)}\) is the velocity of the \(m\)-th particle at time \(t\), \(x_{m}^{(t)}\) is its position, \(pbest_{m}\) is the best position found by that particle to date, and \(gbest\) is the best position found by any particle in the swarm. The position update formula is:  
\[x_{m}^{(t+1)}=x_{m}^{(t)}+v_{m}^{(t+1)} \tag{13}\]  
This design allows particles to adjust their search direction and speed based on their historical best positions and the best positions, effectively exploring the potential optimal solution space while maintaining diversity. In this way, the FastSLAM-PSO can improve the performance of SLAM in complex environments, especially when dealing with high-noise data and large-scale environments. The complete algorithm is shown as Algorithm 1.