## Appendix A Resampling Algorithms
\)\(\triangleright\) Compute Jacobian
7:\(Z_{n}=H_{a}\mathbb{C}_{m}^{m}H_{a}^{T}+R_{n}\)
8:\(H_{p}= \begin{pmatrix}-\frac{dx}{\sqrt{r}}&-\frac{dy}{\sqrt{r}}&0\\ \frac{dy}{\sqrt{r}}&-\frac{dx}{1}\end{pmatrix} \)
9:\(HZH[n]=H_{p}^{T}Z_{n}^{-1}H_{p}\)\(\triangleright\) Shared Memory
10:\(\hat{\rho}=\sqrt{(\hat{x}_{n}-x_{r})^{2}+(\hat{y}_{n}-y_{r})^{2}}\)
11:\(\hat{\beta}=\arctan(\frac{\hat{y}_{n}-y_{r}}{\hat{x}_{n}-x_{r}})\)
12:\(\delta= \begin{pmatrix}\sqrt{r}-\hat{\rho}\\ angle-\hat{\beta}\end{pmatrix} \)
13:threadSynch();
14:endfor
```
**Algorithm A15**\(\Sigma_{n}\) computation
```
1:for\(n=threadId\) to \(N_{c}\), step = \(blockDim\)do
2:if\(n=0\)then
3:\(\Sigma_{minus1}=\Sigma_{0}^{m}\)
4:else
5:\(\Sigma_{minus1}=Signas[n]\)
6:\(\Sigma_{n}^{-1}=HZH[n]+\Sigma_{minus1}^{-1}\)
7:\(Sigmas[n]=\Sigma_{n}\)\(\triangleright\) Shared Memory
8:endif
9:threadSynch();
10:endfor
```
**Algorithm A16**\(\mu\) correction computation
```
1:for\(n=threadId\) to \(N_{c}\), step = \(blockDim\)do
2:\(SHZ=\Sigma_{n}H_{p}^{T}Z_{n}^{-1}\delta\)
3:\(\mu_{0}[n]=SHZ[0]\)\(\triangleright\) Shared Memory
4:\(\mu_{1}[n]=SHZ[1]\)\(\triangleright\) Shared Memory
5:\(\mu_{2}[n]=SHZ[2]\)\(\triangleright\) Shared Memory
6:threadSynch();
7:endfor
```
**Algorithm A17**\(\mu\) correction computation
```
1:if\(threadId==0\)then
2:\(x_{n}^{m}=\mu_{0}[N_{c}-1]\)
3:\(y_{t}^{m}=\mu_{1}[N_{c}-1]\)
4:\(\theta_{t}^{m}=\mu_{2}[N_{c}-1]\)
5:endif
```
**Algorithm A17** Correction Application

### Landmark Estimation
```
1:\(label=z_{n}[3]\)
2:\(particleSeenLandmarks[label]=\) True
3:\(reinitW=\) True \(\triangleright\) Initialize the weights of the particles
4:\(angle=z_{n}[0]+\theta_{r}^{m}\)
5:\(x_{n}^{m}=x_{n}^{m}+z_{n}[0]\cdot\cos(angle)\)\(\triangleright\) Initialize Landmark Position
6:\(y_{n}^{m}=y_{r}^{m}+z_{n}[1]\cdot\sin(angle)\)
7:
8:\(dx=x_{n}^{m}-x_{r}^{m}\)\(\triangleright\) position of the landmark w.r.t. particle pose
9:\(dy=y_{n}^{m}-y_{r}^{m}\)
10:\(\hat{\rho}^{2}=dx^{2}+dy^{2}\)
11:\(\hat{\rho}=\sqrt{\hat{\rho}^{2}}\)\(\triangleright\) Expected Range
12:\(H=\left( \begin{array}{cc}\frac{dx}{\hat{\rho}}&\frac{dy}{\hat{\rho}}\\ -\frac{dy}{\hat{\rho}^{2}}&\frac{dx}{\hat{\rho}^{2}}\end{array} \right)\)\(\triangleright\) Compute Jacobian
13:\(C_{n}^{m}=H^{-1}\cdot Q\cdot H^{-1T}\)
```
**Algorithm A18** Landmark Initialization
```
1:\(dx=x_{n}^{m}-x_{r}^{m}\)\(\triangleright\) position of the landmark w.r.t. particle pose
2:\(dy=y_{n}^{m}-y_{r}^{m}\)
3:\(\hat{\rho}^{2}=dx^{2}+dy^{2}\)
4:\(\hat{\rho}=\sqrt{\hat{\rho}^{2}}\)\(\triangleright\) Expected Range
5:\(angle=\arctan(\frac{dy}{dx})\)
6:\(\hat{\beta}=angle-\theta_{r}^{m}\)\(\triangleright\) Expected Bearing
7:\(H=\left( \begin{array}{cc}\frac{dx}{\hat{\rho}}&\frac{dy}{\hat{\rho}}\\ -\frac{dy}{\hat{\rho}^{2}}&\frac{dx}{\hat{\rho}^{2}}\end{array} \right)\)\(\triangleright\) Compute Jacobian
8:\(K=C_{n}^{m}\cdot H^{T}\cdot(H\cdot C_{n}^{m}\cdot H^{T}+Q)^{-1}\)\(\triangleright\) Kalman Gain
9:\(d\rho=z_{n}[0]-\hat{\rho}\)\(\triangleright\) Innovation
10:\(d\beta=z_{n}[0]-\hat{\beta}\)
11:\(x_{n}^{m}=K[0]\cdot d\hat{\beta}+K[1]\cdot d\hat{\beta}\)\(\triangleright\) Update Landmark Position
12:\(y_{n}^{m}=K[2]\cdot d\hat{\beta}+K[3]\cdot d\hat{\beta}\)
13:\(C_{n}^{m}=(I-K_{n}^{m}H_{a})C_{n}^{m}\)\(\triangleright\) Update Landmark Covariance
14:\(den=\)det(\(2\pi\cdot Q\))
15:\(W_{c}[n]=\frac{1}{den^{2}}\cdot(d\rho\)\(d\beta)\)\(\triangleright\)\(H^{-1}\cdot\left( \begin{array}{cc}d\rho\\ d\beta\end{array} \right)\)\(\triangleright\) Compute local weight
```
**Algorithm A19** Landmark Update
```
1:if\(reinitW\)then
2:\(w_{c}=1/N_{p}\)
3:else
4:\(w_{c}=\) atomicMultiplication(\(W_{c}\))
5:endif
```
**Algorithm A20** Particle Weight Update

### Landmark Estimation
```
1:\(label=z_{n}[3]\)
2:\(particleSeenLandmarks[label]=\) True
3:\(reinitW=\) True \(\triangleright\) Initialize the weights of the particles
4:\(angle=z_{n}[0]+\theta_{r}^{m}\)
5:\(x_{n}^{m}=x_{n}^{m}+z_{n}[0]\cdot\cos(angle)\