In the formulation for EKF SLAM (RVC chapter 6, appendix H) there is a special case when another landmark is discovered.  The state vector update is simple, concatenate the new landmark to the state vector, but the covariance update is more complex.  I’ve never seen this derived so I decided to do it, as much for my own edification as anything else.  It’s an equation heavy PDF that you can get from below.

Download (downloaded: 145)
EKF covariance matrix update for a new landmark
Tagged on: