Data association with ekf?

Given part of the following algorithm in page 217 probabilistic robotics, this algorithm for EKF localization with unknown correspondences

9. for all observed features $z^{i} = [r^{i} \ \phi^{2} \ s^{i}]^{T}$

10.     for all landmarks $k$ in the map $m$ do

11.         $q = (m_{x} - \bar{\mu}_{x})^{2} + (m_{y} - \bar{\mu}_{y})^{2}$

12.         $\hat{z}^{k} = \begin{bmatrix} \sqrt{q} \\ atan2(m_{y} - \bar{\mu}_{y}, m_{x} - \bar{\mu}_{x} ) - \bar{\mu}_{\theta} \\ m_{s} \\ \end{bmatrix}$

13.         $\hat{H}^{k} = \begin{bmatrix} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \\ \end{bmatrix}$

14.         $\hat{S}^{k} = H^{k} \bar{\Sigma} [H^{k}]^{T} + Q$

15.     endfor

16.     $j(i) = \underset{k}{\operatorname{arg\,max}} \ \ det(2 \pi S^{k})^{-\frac{1}{2}} \exp\{-\frac{1}{2} (z^{i}-\hat{z}^{k})^{T}[S^{k}]^{-1} (z^{i}-\hat{z}^{k})\}$

17.     $K^{i} = \bar{\Sigma} [H^{j(i)}]^{T} [S^{j(i)}]^{-1}$

18.     $\bar{\mu} = \bar{\mu} + K^{i}(z^{i}-\hat{z}^{j(i)})$

19.     $\bar{\Sigma} = (I - K^{i} H^{j(i)}) \bar{\Sigma}$

20. endfor

My question is why the second loop ends in the line 15. Shouldn't it end after the line 19. I've checked the errata of this book but nothing about this issue.

The value $S^k$ (innovation variance) needs to be calculated for all landmarks, but the subsequent update steps (post line 16 --- after argmax), need be applied to all of the map, given the landmark update that was selected on line 16 --- the argmax.
There's a notation problem here, $\argmax$ should have a subscript denoting what the search is over. In this case, $\argmax$ is searching over $k$, since $i$ is the current measurement we're trying to associate with each of the $k=1...$ possible landmarks.
• so the line 16 should be included in the second loop. Am I right? About argmax, is it a function that accepts one observed measurement $z^{i}$ and all the predicted measurements $\hat{z}^{1:k}$ and returns the ID of the observed measurement that belongs to a certain landmark? I'm facing a problem with coding this function in Matlab. Any suggestion from programming perspective? I'm done with EKF-Localization with known correspondences however facing difficulties with unknown correspondences. – CroCo Jun 10 '14 at 3:40
• Let's say we have $z^{i} = \{z^{1} \ z^{2} \}$ and $\hat{z} = \{ \hat{z}^{1} \ \hat{z}^{2} \ \hat{z}^{3} \ \hat{z}^{4} \}$ and $map^{i} = \{m^{1} \ m^{2} \ m^{3} \ m^{4} \}$. Let's say $z^{1}$ is the observed measurement of $m^{4}$, so its predicted measurement $\hat{z}^{4}$. argmax is a function that associates $z^{1}$ with $\hat{z}^{4}$ and assigns the ID of $z^{1}$ to be $z^{4}$. Is this correct?? – CroCo Jun 10 '14 at 3:53
• That is correct, argmax(probability of $z^i|map^i$) returns $1$ since we assumed $m^4$ is the landmark that "most likely" was measured. – Josh Vander Hook Jun 10 '14 at 15:29
• thanks for helping me with this. I though it returns the ID not the probability. So, 1 here means $z^{1}$ represents $m^{4}$ 100 %. This might bring another question which is what about if the value 0.9 so we have some uncertainty. Should I set certain threshold to accept or reject this probability? – CroCo Jun 11 '14 at 5:30