## Appendix A Resampling Algorithms
particle pose
4:\(dy=y_{m}^{m}-y_{1}^{m}\)
5:\(\hat{p}^{2}=dx^{2}+dy^{2}\)
6:\(\hat{p}=\sqrt{\hat{p}^{2}}\)\(\triangleright\) Expected Range
7:\(angle=\arctan(\frac{dy}{dx})\)
8:\(\hat{\beta}=angle-\theta_{r}^{m}\)\(\triangleright\) Expected Bearing
9:\(\hat{z}_{n}^{m}= \begin{pmatrix}\hat{\beta}\\ \hat{\beta}\end{pmatrix} \)\(\triangleright\) Shared Memory
10:\(H_{a}= \begin{pmatrix}\frac{dx}{\rho}&\frac{dy}{\rho}\\ \frac{-dy}{\rho^{2}}&\frac{dx}{\rho^{2}}\end{pmatrix} \)\(\triangleright\) Jacobian
11:\(HPH_{n}^{m}=H_{a}\cdot C_{n}^{m}\cdot H_{a}^{T}\)\(\triangleright\) Shared Memory
12:endfor
13:threadsynch()
14:for\(n=threadId\) to N\({}_{I}\), step = \(blockDim\)do
15:for\(k=0\) to N\({}_{o}\)do\(\triangleright\) Sequential
16:\(dz=z_{k}-\hat{z}_{m}^{m}\)\(\triangleright\) Innovation
17:\(S=HPH_{n}^{m}+R\)
18:\(MD_{n,k}^{m}=S^{-1}dz^{2}\)\(\triangleright\) Global Memory
19:if\(MD_{n,k}^{m}<\chi_{2,95}^{2}\)then
20:\(IC_{n,k}^{m}=1\)\(\triangleright\) Global Memory
21:else
22:\(IC_{n,k}^{m}=0\)\(\triangleright\) Global Memory
23:endif
24:endfor
25:endfor
26:threadsynch();
27:endfor
```
**Algorithm A10** Mahalanobis Distance and Individual Compatibility Test
```
for\(m=blockId\) to \(N_{p}\), step = \(gridDim\)do \(cost=0\) for\(k=0\) to \(N_{o}\)do\(\triangleright\) Sequential \(\text{{\tt{for}}}k=0\) to \(N_{o}\)do\(\triangleright\) From Global Memory \(\text{{\tt{threadSynch(}}})\); endfor \(new=N_{o}\)\(\triangleright\) New Label \(next=0\)\(\triangleright\) Auxiliary Variable \(n_{c}=0\)\(\triangleright\) Global Memory \(k=0\) to \(N_{o}\)do\(\triangleright\) Sequential if\(\text{{\tt{not}}}\text{{\tt{matchable}}}_{k}\)then \(h_{k}=new\)\(\triangleright\) Global Memory \(new+=1\) else \(n_{c}+=1\) \(aux_{next}=k\) \(next+=1\) endif \(\text{{\tt{threadSynch(}}})\); endfor endfor
```
**Algorithm A11** Problem Preparation
```
for\(m=blockId\) to \(N_{p}\), step = \(gridDim\)do \(cost=0\) for\(k=0\) to \(N_{o}\)do\(\triangleright\) Sequential \(obsId=aux_{k}\) for\(n=threadId\) to \(N_{l}\), step = \(blockDim\)do if\(\text{{\tt{not}}}\text{{\tt{assigned}}}_{n}\)then \(LB_{n}^{bsld}=cost+MD_{n,obsId}^{m}\) else \(LB_{n}^{bsld}=\text{{\tt{inf}}}\) endif \(\text{{\tt{threadSynch(}}})\); \(\text{{\tt{if}}}LB_{n}^{bsld}<\chi_{22,0}^{3}\)then\(\triangleright\) Joint Compatibility Test \(\text{{\tt{compute}}}\text{{\tt{PC}}}\)\(\triangleright\) Partial Cost  else \(PC=\text{{\tt{inf}}}\) endif \(LB_{n}^{bsld}+=\text{{\tt{inf}}}\) endfor \(\text{{\tt{threadSynch(}}})\); \(\text{{\tt{selectedIdx}}}=\text{{\tt{arg}}}\min(LB)\) \(cost+=MH_{\text{{\tt{selectedIdx}}}}^{m}=0\) endfor endfor
```
**Algorithm A12** Branch and Bound and Joint Compatibility Test

### Proposal Adjustment
```
for\(m=blockId\) to \(N_{p}\), step = \(gridDim\)do\(cost=0\) for\(k=0\) to \(N_{o}\)do\(\triangleright\) Sequential \(obsId=aux_{k}\) for\(n=threadId\) to \(N_{l}\), step = \(blockDim\)do if\(\text{{\tt{not}}}\text{{\tt{assigned}}}_{n}\)then \(LB_{n}^{bsld}=cost+MD_{n,obsId}^{m}\) else \(LB_{n}^{bsld}=\text{{\tt{inf}}}\) endif \(LB_{n}^{bsld}+\text{{\tt{inf}}}\) endfor \(\text{{\tt{threadSynch(}}})\); \(cost+=MH_{\text{{\tt{selectedIdx}}}}^{m}=0\) endfor
```
**Algorithm A13** Initialization \(\Sigma_{0},\mu_{0}\)

### Proposal Adjustment
```
for\(m=blockId\) to \(N_{p}\), step = \(gridDim\)do\(cost=0\) for\(k=0\) to \(N_{o}\)do\(\triangleright\) Sequential \(obsId=aux_{k}\) for\(n=threadId\) to \(N_{l}\), step = \(blockDim\)do if\(\text{{\tt{assigned}}}_{n}\)then \(LB_{n}^{bsld}=cost+MD_{n,obsId}^{m}\) else \(LB_{n}^{bsld}=\text{{\tt{inf}}}\) endif \(LB_{n}^{bsld}+\text{{\tt{inf}}}\) endfor \(\text{{\tt{threadSynch(}}})\); \(cost+=MH_{\text{{\tt{selectedIdx}}}}^{m}=0\) endfor
```
**Algorithm A14** Initialization \(\Sigma_{0},\mu_{0}\)
```
1:for\(n=threadId\) to \(N_{c}\), step = \(blockDim\)do
2:\(dx=x_{n}-x_{r}\)
3:\(dy=y_{n}-y_{r}\)
4:\(angle=\arctan(\frac{dy}{dx}-\theta)\)
5:\(r=(x_{l}-x_{r})^{2}+(y_{l}-y_{r})^{2}\)
6:\(H_{a}= \begin{pmatrix}\frac{dx}{\sqrt{r}}&\frac{dy}{\sqrt{r}}\\ -\frac{dy}{\sqrt{r}}&\frac{dx}{\sqrt{r}}\end{pmatrix}