1
$\begingroup$

I've recently been learning about SLAM and EKF-SLAM.

I've began my implementation in python, but have had trouble managing the updating of P, especially when it comes to adding new landmarks. Currently there is no 'P' but just a few separate matrices that I have to stitch together when needed.

My implementation can be seen here: http://pastebin.com/r7wUMgY7

How best should I manage the large covariance matrix, should I be using one matrix, like the algorithm suggests? Thanks in advance.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

You must use one matrix, because you must manage the cross covariances. The best way to add a landmark is to expand the matrix, by copying it into a larger one (with two extra rows for x,y and two extra columns for the same). Then, you have to initialize all the off-diagonal terms using the jacobian matrix. Similarly, the state vector must be expanded to include the new landmark as well.

This answer shows how to initialize a new landmark in this way. Note the matrix shown there is block represented. It could in fact be very large.

Now how you represent that matrix is up to you. However, keeping many small matrices that you stitch together is no more efficient (cpu or memory) than keeping one large one.

Improve this answer
$\endgroup$
1
  • $\begingroup$ Fair point and thanks for the link. I'll most likely go for the 'initialise big empty matrix' approach. $\endgroup$
    – jub
    Commented Mar 29, 2015 at 23:22

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.