Algorithm 4.

Algorithm of the EKF-SLAM based on the eigenvalues selection approach.

1:	Let Nt be set of the observed features at time t
2:	Let Mt ⊆ Nt be the set of features with correct association at time t
3:	Let LIM be the maximum number of features to be used in the correction stage
4:	forj = 1 to min{LIM, ⧣Mt} do
5:	find $z_j^{\mathit{opt}}$: argzmin(|Pt,j|)
6:	$K_{t,j}=P_{t,j}^{-}{H}_{t,j}^T{(H_{t,j}{P}_{t,j}^{-}{H}_{t,j}^T+R_{t,j})}^{-1}|z_j^{\mathit{opt}}$
7:	${\widehat{\xi }}_{t,j}={\widehat{\xi }}_{t,j}^{-}+K_{t,j}(z_j^{\mathit{opt}}-h({\widehat{\xi }}_{t,j}^{-}))$
8:	$P_{t,j}=(I-K_{t,j}{H}_{t,j}))P_{t,j}^{-}|z_j^{\mathit{opt}}$
9:	$M_t=M_t-\{z_{t,j}^{\mathit{opt}}\}$
10:	$P_{t,j}^{-}:=P_{t,j};{\widehat{\xi }}_{t,j}^{-}={\widehat{\xi }}_{t,j}$
11:	end for