3
$\begingroup$

I am trying to implement an EKF-SLAM using the algorithm for unknown correspondences proposed in the book "Probalistic Robotics" by Sebastian Thrun in Table 10.2 .

By now I understand actually all of the algorithm except of the initialization of new landmarks in the covariance matrix $ P_{new} $.

In that algorithm when a new landmark is detected the procedure is just the same as if a normal measurment update for an already observed landmark is done: the Kalman gain $ K $ is calculated for the new landmark and then the covariance is updated with that Kalman gain and the jacobian $ H $ of that new landmark like this $ P_{new}= (I - K * H) * P$ .

In my understanding a just new observed landmark would not have any effect on the rows and columns that correspond to already mapped landmarks or the robot pose in the covariance matrix. Instead I think that just two rows and columns for x and y should be created with some uncertainity like proposed here: the uncertainty of initializing new landmark in EKF-SLAM .

I tried to split down the calculation of $ P_{new}$ via claculating it blockwise to see if I could somehow come to the same initialization as shown in the link above. But I end up having a different covariance matrix where apparently the new landmark is effecting the rows and columns of the parts of the old covariance, which in my view can't be right.

I hope I don't understand the pseudo code of the book wrong or I did a mistake in my try to come to the same initialization. Any advice how the initialization of new lnndmarks work in that code or if it actually is the same as in the link will be appreciated.

Edit

So basically what I am asking is: why would they do a normal Kalman update of the covariance matrix in line 24 of table 10.2 for a new observed landmark? Why is there no explicit case for the initialization of new rows/columns of new observed landmarks in the covariance matrix? It seems to me like they just do a normal measurement update even for a just newly observed landmark.

$\endgroup$
1
$\begingroup$

I explained exactly how you can add the new row/columns to the covariance matrix in this answer.

$\endgroup$
0
$\begingroup$

Unfortunately, the aforementioned book doesn't address initializing the uncertainty of a new landmark, however, it does address initializing the mean of a new landmark though (i.e. by using the inverse measurement function). In this link, the author addresses the issue. See 2.3.4 Landmark initialization for full observations in the pdf. The bottom line is that you should never ever touch the elements of the uncertainty matrix. The proper way to augment the uncertainty of a new landmark is addressed in the aforementioned pdf.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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