Algorithm 1 Step-by-Step Implementation of the Proposed Transition Model
Require: vehicle states $X_{\mathrm{V},k}$, $Y_{\mathrm{V},k}$, ${\varphi }_k$, and particles ${\left\{{\omega }_{i,k}^{(\ell )},{\mathbf{\beta }}_{i,k}^{(\ell )}\right\}}_{i=1}^{N^{(\ell )}}$ representing the distribution of each plane with label ℓ, for all labels $\ell \in \mathbb{L}$
1:  compute matrix ${\mathit{R}}_k$	▹ use Equation (28)
2:  generate a noise sample $\mathit{u}\sim \mathcal{N}(0,\Sigma )$	▹$\Sigma \sim$ diag(noise variances)
3:  generate $X_{\mathrm{V},k+1}$, $Y_{\mathrm{V},k+1}$ and ${\varphi }_{k+1}$	▹ use Equation (36)
4:  compute matrix ${\mathit{R}}_{k+1}$	▹ use Equation (30)
5:  ${\mathit{M}}_k^{-1}\leftarrow {\mathit{R}}_k^{-1}{\mathit{R}}_{k+1}$
6:  for $\ell \in \mathbb{L}$ do
7:  for $i⩽N^{(\ell )}$ do
8:     ${{\mathbf{\beta }}_{i,k+1}^{(\ell )}}^{\mathrm{T}}\leftarrow {{\mathbf{\beta }}_{i,k}^{(\ell )}}^{\mathrm{T}}\times {\mathit{M}}_k^{-1}$
9:     ${\omega }_{i,k+1}^{(\ell )}\leftarrow {\omega }_{i,k}^{(\ell )}$
10:  end for
11:  ${\displaystyle {\widehat{\mathbf{\beta }}}_{k+1}^{(\ell )}\leftarrow \sum _{i=1}^{N^{(\ell )}}{\omega }_{i,k+1}^{(\ell )}{\mathbf{\beta }}_{i,k+1}^{(\ell )}}$	▹ compute the EAP estimate
12:  end for