## 2 Simultaneous Localization and Mapping
### FastSLAM 2.0  
FastSLAM 2.0, like the other PF-based approaches, provides the robot's and landmarks' pose estimations as discrete samples (particles) of a probabilistic distribution. This characteristic allows for overcoming limitations brought by modeling the robot's pose as Gaussian distributed, as in EKF-based methods [21]. However, the map features are still modeled as Gaussian distributed. The algorithm leverages the conditional independence of map features and the robot's path, as expressed in the following:  
\[p(X_{t},m\mid z_{t})=P(X_{t}\mid z_{t})P(m\mid X_{t},z_{t})\]  
where \(X_{t}\) is the set of the robot poses \((x_{i},y_{i},\theta_{i})\), \(i=1,\ldots,t\) from the beginning of the navigation until time \(t\), \(m\) is the set of the map features coordinates (called _landmarks_) \((x_{lj},y_{lj})\), \(j=1,\ldots,N_{l}\) (\(N_{l}\) is the number of landmarks), and \(z_{t}\) is the set of observations \((\rho_{k},\beta_{k})\), \(k=1,\ldots,N_{0}\) (\(N_{0}\) is the number of observed features). As described in Section 4.1, the robot is modeled as a rigid body navigating a planar space. Therefore, \((x_{i},y_{i})\) and \((x_{ij},y_{j})\) are the Cartesian coordinates of the robot and landmark \(j\), expressed in meters, and \(\theta_{i}\) is the orientation of the robot, expressed in radians. As for the measurement, for the sake of our dissertation, we assume that the robot is equipped with a generic range sensor. Consequently, the measurements \(\rho_{k}\) and \(\beta_{k}\) indicate the detections' range (meters) and the bearing (radians). Therefore, the joint distribution of the full path \(X_{t}\) and the map \(m\) can be obtained, according to Bayes' Theorem, with the multiplication of the probability of the trajectory \(X_{t}\) given the last measurement \(z_{t}\) by the probability of the map \(m\) given the path \(X_{t}\) and the measurement \(z_{t}\). This consideration allows FastSLAM 2.0 to decouple the map features from the robot poses via a factorization called Rao-Blackwellization, where \(P(X_{t}|z_{t})\) is expressed through particles, i.e., a set of weighted hypotheses on the robot's trajectory. Consequently, a different map is estimated for each particle, i.e., for each separate path estimation.  
In other words, by treating each particle as the true trajectory, we solve \(N_{p}\), the mapping with known poses problems, in which all map features are uncorrelated. This enables independent processing of all the feature measurements. In more detail, each map feature is estimated through an EKF, which means that given \(N_{p}\) particles and \(N_{l}\) map features, we have \(N_{p}\times N_{l}\) different EKFs.  
The algorithm pipeline can be subdivided into steps, as summarized in Figure 1. In more detail:  
* _Prediction_, where the robot pose is predicted based on the motion commands and a motion model (Section 2.1.1);
* _Data Association_, where the observations are matched with the previously stored map features. This step is not performed if the observations are assumed to be already labeled (_known data association_). In this work, we address the _unknown data association_ case (Section 2.2);
* _Proposal Adjustment_, where the robot's pose estimation is adjusted based on the matched observations. This step marks the difference between FastSLAM 1.0 and FastSLAM 2.0 and ensures better robustness in the pose estimate whenever the proprioceptive sensors have higher noise levels than the exteroceptive sensors (Section 2.1.3);
* _Landmark Estimation_, where the pose estimations of the observed landmarks are updated in an EKF-like fashion according to the observations. At this step, weights are assigned to each particle (Section 2.1.4);
* _Importance resampling_, where particle estimations can be maintained or deleted with a probability proportional to their weight. The weight is calculated within the _Landmark Estimation_ step. Several strategies are available in the state-of-the-art, allowing for different trade-offs between accuracy and execution time (Section 2.1.5).