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.
