Algorithm 1 Proposed localization algorithm.

At $t=1$, initialize $p\left(x_1|Z_1\right)$ using (7). At each $t>1$: 1. Compute ${\lambda }_t$ and ${\Lambda }_t$ using (18) and (19). 2. For each $j=1,\cdots ,I$, do: (a) Pick an index $i\in \left\{1,\cdots ,I\right\}$ by drawing it from the discrete distribution such that i has probability proportional to ${\varpi }_{t-1|t-1}^i{\alpha }_t^i$. (b) Draw $y_{t|t}^j$ from $\mathcal{N}\left(y_t;{\xi }_t^i,{\Xi }_t^i\right)$. (c) Compute the particle weight using ${\varpi }_{t|t}^j=\frac{p\left(y_{t|t}^j|Z_t\right)}{{\tilde{p}}_t\left(y_{t|t}^j\right)}$. (d) Pick an index $i\in \left\{1,\cdots ,I\right\}$ by drawing it from the discrete distribution such that i has probability proportional to ${\psi }_t^i\left(y_{t|t}^j\right)$. (e) Draw ${\theta }_{t|t}^j$ from $\mathcal{N}\left({\mu }_t^i\left(y_{t|t}^j\right),{\rho }_t^i\left(y_{t|t}^j\right)\right)$. (f) Put $x_{t|t}^j={\left[y_{t|t}^{j\top },{\theta }_{t|t}^j\right]}^{\top }$. 3. Normalize the weights so that ${\sum }_{j=1}^I{\varpi }_{t|t}^j=1$. 4. Estimate the position using ${\widehat{x}}_t={\sum }_{i=1}^I{\varpi }_{t|t}^i{x}_{t|t}^i$.