## 2 Simultaneous Localization and Mapping
### Data Association  
This section is dedicated to a more detailed discussion about the data association step. First, we provide a state-of-the-art overview of the probabilistic data association techniques, focusing on the FastSLAM implementation. Then, Section Joint Compatibility Branch and Bound (JCBB) method and its fundamental characteristics to provide the reader with sufficient background on the methodology to understand the following sections of this paper. The Joint Compatibility Branch and Bound (JCBB) algorithm [47] was selected to carry out the data association task in our implementation. JCBB can handle multiple observations simultaneously and reduces the number of possible observation-landmark matches by performing a chi-square compatibility test based on the joint quadratic Mahalanobis distance between the observations and the landmarks. After the joint compatibility test is performed, the feasible pairings are evaluated and selected according to the branch-and-bound method. The validity of using a joint compatibility test within the data association step has been demonstrated in [47]. To the best of our knowledge, JCBB has been implemented in the state-of-the-art in combination with the Extended Kalman Filter (EKF) algorithm and not with the Particle Filter (PF) algorithm, which is the basis of FastSLAM 2.0. Since the Mahalanobis distance computation is subject to the assumptions characterizing each estimation method (EKF in the state-of-the-art, PF in our work), it has to be derived accordingly, as shown below. The Mahalanobis distance computed for hypothesis \(H_{i}\) is expressed as follows:  
\[D_{H_{i}}^{2}=h_{H_{i}}^{\mathrm{T}}S_{H_{i}}^{-1}h_{H_{i}} \tag{3}\]  
where \(h_{H_{i}}\) is the difference between the observations and the expected measurement and \(C_{H_{i}}\) is given by the following:  
\[S_{H_{i}}=H_{H_{i}}PH_{H_{i}}^{\mathrm{T}}+G_{H_{i}}RGH_{i}{}^{\mathrm{T}} \tag{4}\]  
where \(P\) is the estimation error covariance function for the robot state and the landmark position and \(H\) is a matrix of Jacobians discussed later. \(G\) is the matrix of Jacobians of the measurement model with respect to the observations. Given our measurement model and motion model described in Equation (1) and in Algorithm 1, \(G\) is an identity matrix. \(R\) is the measurement noise covariance matrix. This implies the following:  
\[G_{H_{i}}RG_{H_{i}}^{T}=R\]  
The first element of Equation (4) has yet to be computed. In the implementations available in the state-of-the-art, which are related to EKF-SLAM, the covariance matrix \(P\) looks like Equation (5), where on the main block diagonal we can find the covariance matrix of the robot pose estimation error \(Cov_{r}\) and the landmark pose estimation error covariance matrices \(Cov_{i_{1}}\), and the generic extra-diagonal elements \(A_{i,j}\) represent the correlation between the robot pose and the estimated landmark position (all the \(A_{i,1}\), which are non-zero in the EKF), and the landmarks among themselves (zero in any case). However, since in the PF, the landmarks and the robot pose are not correlated conditionally to each particle, \(P\) is a block diagonal matrix. Furthermore, since Rao-Blackwallization allows for estimating the landmark poses conditionally to the robot pose, \(Cov_{r}\) is also null conditionally to each particle. Therefore, \(P\) is a block diagonal matrix, which is also symmetrical by construction, as in the following:  
\[P= \begin{bmatrix}Cov_{r}&A_{1,2}&A_{1,3}&\cdots&A_{1,N}\\ A_{2,1}&Cov_{l_{0}}&A_{2,3}&\cdots&A_{2,N}\\ A_{3,1}&A_{3,2}&Cov_{l_{1}}&\ddots&\vdots\\ \vdots&\vdots&\ddots&\ddots&A_{N-1,N}\\ A_{N,1}&A_{N,2}&\cdots&A_{N,N-1}&Cov_{l_{n}}\end{bmatrix}  \tag{5}\]  
On the other hand, \(H\) generically looks like the matrix below:. \[H= \begin{bmatrix}H_{p,0}&H_{a,0}&A_{1,2}&A_{1,3}&\cdots&A_{1,N}\\ H_{p,1}&A_{2,1}&H_{a,0}&A_{2,3}&\cdots&A_{2,N}\\ H_{p,2}&A_{3,1}&A_{3,2}&H_{a,0}&\ddots&\vdots\\ \vdots&\vdots&\vdots&\ddots&A_{N-1,N}\\ H_{p,N}&A_{N,1}&A_{N,2}&\cdots&A_{N,N-1}&H_{a,0}\end{bmatrix}