Robotics Stack Exchange is a question and answer site for professional robotic engineers, hobbyists, researchers and students. It only takes a minute to sign up.
Sign up to join this community
I am trying to implement GraphSlam. I go through this paper http://robots.stanford.edu/papers/thrun.graphslam.pdf. I have some doubt about the algorithm. I attached a picture with this question
My question is that what is the need to augmented a identity matrix in line no 7 and 8 of this algorithm.
Please help to clear the doubt.
In GraphSLAM, the information matrix $\Omega$ and the information vector $\xi$ are given directly by the log of the full posterior via the following expression:
Therefore we need to derive an expression for our full posterior which looks like Eq 26 above so that we can identify $\Omega$ and $\xi$.
This is done in sections 5-1 to 5-3 of the paper, and results in this ugly expression for the full posterior:
Even if you don't fully understand this expression, you can see that it includes two sums: one over $t$ and the other one over $i$. The first sum corresponds to the constraints resulting from controls/odometry, whereas the second corresponds to the constraints resulting from landmark measurements. The lines 7 and 8 of the algorithm you posted above simply add the contribution of one control/odometry constraint to $\Omega$ and $\xi$. You can see that they are inside a loop iterating over all the $t$ controls, which corresponds exactly to our first sum in equation 25. In lines 18 and 19 of the algorithm (not visible on your image) you can see the same thing being done for the measurement constraints.
And this is how we build $\Omega$ and $\xi$.
