# Dynamic Indoor Localization Using Maximum Likelihood Particle Filtering
## 4 Proposed Algorithm
In view of (12), this can be done by drawing samples from \(p(\theta_{t}|y_{t},Z_{t})\) and \(p(y_{t}|Z_{t})\). It follows from (16) that \(p(\theta_{t}|y_{t},Z_{t})\) is a Gaussian mixture distribution. Hence, we can readily draw samples from it. On the other hand, from (13) and (15) we obtain  
\[p(y_{t}|Z_{t})\propto p(z_{t}|y_{t})\sum_{i=1}^{l}\psi_{t}^{i}(y_{t}),\]  
from where it is not clear how to draw samples. Our strategy to go around this consists finding a convenient Gaussian approximation \(\tilde{p}_{t}(y_{t})\) of \(p(y_{t}|Z_{t})\), from where we can draw samples, and then weight these samples to account for the difference between \(\tilde{p}_{t}(y_{t})\) and \(p(y_{t}|Z_{t})\). To derive the Gaussian approximation \(\tilde{p}_{t}(y_{t})\) we use (13) and proceed in three steps. In Lemma 3 we do a Gaussian approximation of \(p(y_{t}|Z_{t-1})\), in Lemma 4 we do the same with \(p(z_{t}|y_{t})\), and in Lemma 5 we combine these two approximations to obtain \(\tilde{p}_{t}(y_{t})\). **Lemma 3**. : _If (10) holds, then_  
\[p(y_{t}|Z_{t-1})\simeq\sum_{i=1}^{l}\varpi_{t-1|t-1}^{i}\mathcal{N}\Big{(}y_{ t};\tilde{g}_{t-1}^{i}(0),\mathbf{J}g_{t-1}^{i}(0)Q_{e}\mathbf{J}g_{t-1}^{i \top}(0)\Big{)},\]  
_where \(Q_{e}=SQS^{\top}\) with \(S=[\mathbf{I}_{2},0]\) and \(\mathbf{I}_{2}\) denoting the \(2\times 2\) identity matrix._  
**Lemma 4**. : _The following approximation holds_  
\[p(z_{t}|y_{t})\stackrel{{\sim}}{{\sim}}\mathcal{N}(y_{t};\lambda_{t},\Lambda_{t}), \tag{17}\]  
_where_  
\[\lambda_{t} =\underset{y\in\mathbb{R}^{2}}{\arg\max}\Xi_{t}(y), \tag{18}\] \[\Lambda_{t} =-\Big{[}\
abla^{2}\Xi_{t}(\lambda_{t})\Big{]}^{-1}. \tag{19}\]  
_with_  
\[\Xi_{t}(y)=\log\mathcal{N}(z_{t};m(y),R).\]  
Combining Lemmas 3 and 4 we obtain the Gaussian approximation of \(p(y_{t}|Z_{t})\) given in the following lemma. **Lemma 5**. : _The following approximation holds_  
\[p(y_{t}|Z_{t})\simeq\tilde{p}_{t}(y_{t}),\]  
_where_  
\[\tilde{p}_{t}(y_{t})=\sum_{i=1}^{I}\alpha_{t-1|t-1}^{i}\alpha_{t}^{i}\mathcal{ N}\Big{(}y_{t};\tilde{\xi}_{t}^{i},\Xi_{t}^{i}\Big{)},\]  
_with_  
\[\alpha_{t}^{i} =\mathcal{N}\Big{(}0;g_{t-1}^{i}(0)-\lambda_{t},\Lambda_{t}+ \mathbf{J}g_{t-1}^{i}(0)Q_{\mathcal{L}}\mathbf{J}g_{t-1}^{i\top}(0)\Big{)},\] \[\Xi_{t}^{i} =\Big{[}\Lambda_{t}^{-1}+\Big{(}\mathbf{J}g_{t-1}^{i}(0)Q_{ \mathcal{L}}\mathbf{J}g_{t-1}^{i\top}(0)\Big{)}^{-1}\Big{]}^{-1},\] \[p(y_{t}|Z_{t-1})\simeq\sum_{i=1}^{l}\varpi_{t-1|t-1}^{i}\mathcal{N}\Big{(}y_{ t};\tilde{g}_{t-1}^{i}(0),\mathbf{J}g_{t-1}^{i}(0)Q_{e}\mathbf{J}g_{t-1}^{i \top}(0)\Big{)},\]0  
Combining the results in Lemmas 1, 2 and 5 we can build the desired approximation (11) following the procedure described in Algorithm 1. At \(t=1\), initialize \(p(x_{1}|Z_{1})\) 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: 1. Pick an index \(i\in\{1,\cdots,I\}\) by drawing it from the discrete distribution such that \(i\) has probability proportional to \(\varpi_{t-1|t-1}^{j}\alpha_{t}^{i}\). 2. Draw \(y_{t|t}^{j}\) from \(\mathcal{N}\big{(}y_{t};\hat{\xi}_{t}^{i},\Xi_{t}^{i}\big{)}\). 3. Compute the particle weight using \(\varpi_{t|t}^{j}=\frac{p\big{(}y_{t|t}^{j}|Z_{t}\big{)}}{\tilde{p}_{t}\big{(} y_{t|t}^{j}\big{)}}\). 4. Pick an index \(i\in\{1,\cdots,I\}\) by drawing it from the discrete distribution such that \(i\) has probability proportional to \(\psi_{t}^{i}\Big{(}y_{t|t}^{j}\Big{)}\). 5. Draw \(\theta_{t|t}^{j}\) from \(\mathcal{N}\Big{(}\mu_{t}^{i}\Big{(}y_{t|t}^{j}\Big{)},\rho_{t}^{i}\Big{(}y_ {t|t}^{j}\Big{)}\Big{)}\). 6. Put \(x_{t|t}^{j}=\Big{[}y_{t|t}^{\top},\theta_{t|t}^{j}\Big{]}^{\top}\). 3. Normalize the weights so that \(\sum_{j=1}^{I}\varpi_{t|t}^{j}=1\). 4. Estimate the position using \(\hat{x}_{t}=\sum_{t=1}^{I}\varpi_{t|t}^{j}x_{t|t}^{i}\).