EKF localization known correspondences

Question

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.

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.