I'm facing problems with this book and it is the only book that discusses localization in depth. The results that I'm getting makes no sense. I've read a lot of papers, majority of them copy the localization algorithm from this book. My question here is why $\bar{\mu}$ and $\bar{\Sigma}$ are being changed every iteration?? I'm using them to get the predicted measurements in lines 11- 13, so they should be fixed.

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

10.   $j = c^{i}$

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

12.         $\hat{z}^{i} = \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}^{i} = \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}^{i} = H^{i} \bar{\Sigma} [H^{i}]^{T} + Q $

15.     $K^{i} = \bar{\Sigma} [H^{i}]^{T} [S^{i}]^{-1}$

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

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

18. endfor

19. $\mu = \bar{\mu}$

20. $\Sigma = \bar{\Sigma}$

Please suggest me other books that discuss EKF localization in depth.


1 Answer 1


The underlying assumption seems to be that if all observed features you measure at the same time are independent, you can apply the EKF correction step several times: Once for each observed feature. (I am currently not completely sure whether this is valid.)

This is what the above algorithm does. It is an optimization of the naive solution, which would work like this:

You can also combine the measurement vectors of all observed features to a full measurement vector $z$ by stacking them on top of each other:

$z = (z_0^T, z_1^T, z_2^T, ...)^T$

You will get the full Jacobian by stacking all Jacobians accordingly. The full measurement covariance matrix $Q$ is the block-diagonal matrix which contains all individual measurement covariances $Q$ as blocks on its diagonal.

This way, you can use the original EKF algorithm which modifies $\bar\mu$ and $\bar\Sigma$ only once.

But: This naive solution is computationally more expensive, which is why you should use the method described in Probabilistic Robotics. Using the naive method, you have to invert one much bigger matrix $H \bar\Sigma H^T + Q$ instead of several much smaller versions.

  • $\begingroup$ this is not answering my question. I'm asking here about the predicted measurements. Why $\bar{\mu}$ and $\bar{\Sigma}$ are being modified every iteration? To me, this affects the innovation step ($z^{i} - \hat{z}^{i}$). $z^{i}$ is acquired from the actual pose, and thus $\hat{z}^{i}$ should be acquired from the estimated pose. $\endgroup$
    – CroCo
    Commented Jun 18, 2014 at 19:02

Your Answer

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

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