Algorithm 1 Non-Gaussian Delayed Particle Smoother (nGDPS) for Vehicle Localization

% Initialization:

At time $k=0$:

Set $X_0={\left[po{s}_0,v_0,a_0\right]}^{\prime }$, the state vector representing the initial information of vehicle where $po{s}_0$ , $v_0$, and $a_0$ are position, velocity, and acceleration respectively;

Select initial covariance matrices $R_0$ and $Q_0$ related to the measurement and process noises respectively;

Draw M particles and set the weight $w_0\left(X_0^i\right)=\frac{1}{M}$, $i=1,\cdots ,M$ given that the prior knowledge $X_0^i~p\left(X_0^i\right)$;

Set the fixed-delay size L and sample time K.

For each time instant $k=1,\dots ,K$ do

For each particle $i=1,\dots ,M$ do % Importance sampling: Compute the state of Λ particles where Λ is the subset of particles around the vehicle state $X_{k+L}$ at time $k+L$ (i.e., $\Lambda \subseteq M$); ${\left\{X_{k+L-1}^i\right\}}_{j=1}^{\Lambda }\leftarrow X_{k+L}^{j,i}~q_{EnKF}\left(X_{k+L}^{j,i},X_{k+L}^i|X_{k+L-1}^i,Y_{k+L}\right)$, where $q_{EnKF}(\cdot )$ stands for the importance function defined using EnKF according to Equation (13); For each $l=0,\dots ,L$ do Update the process and measurement noise densities based on Equations (2a) and (2b) The definite scale matrices in terms of the associated covariance matrices are given by $D_{k+l}^i=\frac{\nu -2}{\nu }{R}_{k+l}^i$ and $G_{k+l}^i=\frac{\nu -2}{\nu }{Q}_{k+l}^i$, ($\nu \ne 0$) respectively. % Importance weight update: Compute new weight according to Equation (14); End For  % Normalization:  The normalized weight is given by ${\tilde{w}}_{k+L}^i\leftarrow w_{k+L}^i\ast {\left({\displaystyle \sum _{j=1}^{\Lambda }{w}_{k+L}^j}\right)}^{-1}$, and the state of the vehicle at time k + L is provided by: ${\stackrel{⌣}{X}}_{k+L}\leftarrow {\stackrel{⌣}{X}}_{k+L}+X_{k+L}^i*{\tilde{w}}_{k+L}^i$. % Resampling:  Compute the effective sample size ${\widehat{\lambda }}_e$ as defined in [46]; If ${\widehat{\lambda }}_e<\overline{\lambda }=\frac{2\Lambda }{3}$ (the predefined threshold) then Resample using the Delayed Gibbs sampling (DGS) method, otherwise, % Output the smoothed estimation  Compute the smoothed state estimates of the vehicle: ${\stackrel{⌢}{X}}_{k+L}$ End If

End For

End For