2
$\begingroup$

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.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

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.

FYI, argmax searches over the list of landmarks for the landmark maximizing the equation given. It selects the index of the landmark which is most likely to be the landmark given the measurement value. Notice that the equation in line 16 is the gaussian multivariate pdf with the measurement and mean measurement for landmark k, normalized by the measurement variance, S.

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.

Improve this answer
$\endgroup$
9
  • $\begingroup$ 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. $\endgroup$
    – CroCo
    Jun 10, 2014 at 3:40
  • $\begingroup$ 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?? $\endgroup$
    – CroCo
    Jun 10, 2014 at 3:53
  • $\begingroup$ 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. $\endgroup$
    – Josh Vander Hook
    Jun 10, 2014 at 15:29
  • $\begingroup$ 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? $\endgroup$
    – CroCo
    Jun 11, 2014 at 5:30
  • 1
    $\begingroup$ Yup, seems legit. $\endgroup$
    – Josh Vander Hook
    Jun 12, 2014 at 17:18

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.