Algorithm 1: The proposed scheme.
Require: Σk−1,μk−1,uk,IDL,Rate
1:	[ ${\text{Σ}}_k^{-}$, ${\mu }_k^{-}$ ]= EKF_Predict(Σk−1,μk−1,uk), expressions (11)(12)
2:	{zk} = MeasurementGathering(IDL,Rate), in Section 4
3:	[Σk,μk] = EKF _U pdate( ${\sum }_k^{-}$, ${\mu }_k^{-}$, {zk}), expressions (14)(15)
4:	[IDL,Rate] = SLAM_Supervisor(Σk), in Section 5
5:	return Σk,μk, IDL, Rate