## 2 Related Works
### FastSLAM  
FastSLAM is a probabilistic method that combines PF and the EKF to address the problem of SLAM in mobile robots navigating unknown environments. The key innovation of FastSLAM is the decomposition of the SLAM problem into two independent subproblems: the estimation of the pose of the robot and the landmark positions in the environment [5]. FastSLAM formulates the joint posterior distribution of the pose of robot \(s_{t}\) and the landmarks \(\Theta=\{u_{k},\Sigma_{k}\}\) as  
\[p(\Theta,s_{t}|z_{1:t},u_{1:t})=p(s_{t}|z_{1:t},u_{1:t})\prod_{k=1}^{N}p(\mu_{ k},\Sigma_{k}|z_{1:t},s_{1:t}) \tag{3}\]where \(z_{1x}\) represents all observations from time step 1 to \(t\), and \(u_{1:t}\) represents the control inputs over the same period. \(\mu_{k}\) and \(\Sigma_{k}\) denote the position and covariance matrix of landmark \(k\), respectively. This decomposition shows that the estimation of the pose \(s_{t}\) is independent of the individual landmark estimates \(\mu_{k}\), significantly reducing computational complexity. In FastSLAM, PF is employed to estimate the robot's distribution of pose. PF approximates the posterior distribution using a set of weighted particles \(s_{t}^{[m]}\), where each particle contains an estimate of the trajectory and the positions and covariance estimates of landmarks. At time step \(t\), the pose \(s_{t}^{[m]}\) of each particle is predicted using the motion model \(p\) (\(s_{t}\,|\,u_{t},s_{t-1}\)) as  
\[s_{t}^{[m]}\,\sim\,p(s_{t}\big{|}u_{t},s_{t-1}^{[m]}) \tag{4}\]  
where \(s_{t}^{[m]}\) represents the predicted pose of the \(m\)-th particle at time \(t\). The motion model is typically represented as  
\[s_{t}=f(s_{t-1},u_{t})+\epsilon_{t} \tag{5}\]  
where \(f(\cdot)\) is the motion model function, and \(\epsilon_{t}\) is Gaussian noise representing system uncertainty. Each particle then updates the estimated landmark positions using the Extended Kalman Filter. For the \(m\)-th particle, the position estimate \(\mu_{t}^{[m]}\) of landmark \(k\) is updated as follows  
\[\mu_{k}^{[m]}=\mu_{k}^{[m]}+K_{t}^{[m]}(z_{t}-h(s_{t}^{[m]},\mu_{k}^{[m]})), \tag{6}\]  
where \(h(s_{t}^{[m]},\mu_{t}^{[m]})\) is the observation model that relates the pose of robot \(s_{t}^{[m]}\) to the landmark position \(\mu_{t}^{[m]}\). The Kalman gain \(K_{t}^{[m]}\) is calculated by  
\[K_{t}^{[m]}=\Sigma_{k}^{[m]}H_{t}^{\top}(H_{t}\Sigma_{k}^{[m]}H_{t}^{\top}+R_ {t})^{-1}, \tag{7}\]  
where \(H_{t}\) is the Jacobian of the observation model, and \(R_{t}\) is the observation noise covariance matrix. The weight \(w_{t}^{[m]}\) of each particle is then computed based on the likelihood of the observations given the predicted pose and landmark estimates  
\[w_{t}^{[m]}=p(z_{t}\big{|}s_{t}^{[m]},\mu_{k}^{[m]}), \tag{8}\]  
where the weight represents the agreement between the particle and the actual observations. To prevent particle degeneration, FastSLAM performs resampling based on the weights \(w_{t}^{[m]}\), favoring particles with higher weights while replacing those with lower weights. This maintains the particle diversity and robustness of the algorithm. The EKF is used to update the position of landmarks within each particle. For particle \(s_{t}^{[m]}\), the covariance matrix of the landmark positions is updated using the process model  
\[\Sigma_{k}^{[m]}=F_{t}\Sigma_{k,t-1}^{[m]}F_{t}^{\top}+Q_{t}, \tag{9}\]  
where \(F_{t}\) is the Jacobian of the motion model, and \(Q_{t}\) is the process noise covariance matrix. The updated covariance matrix is then calculated as  
\[\Sigma_{k}^{[m]}=(I-K_{t}^{[m]}H_{t})\Sigma_{k}^{[m]}. \tag{10}\]  
This update step adjusts the landmark position estimates based on the difference between the predicted and observed values, weighted by the Kalman gain. Compared to traditional EKF-SLAM, FastSLAM offers significant computational advantages. In EKF-SLAM, the complexity of jointly estimating both the robot pose and all landmark positions is \(O(N^{2})\), where \(N\) is the number of landmarks.