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.