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

enter image description here

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.

  • $\begingroup$ It wouldn't hurt took at this youtu.be/nLEbJZFm5-E its part of udacity's artificial intelegence for robots free course $\endgroup$ Jul 19, 2018 at 23:09
  • $\begingroup$ I watch those videos. There is no explanation of identity matrix argumentation. $\endgroup$
    – Encipher
    Jul 21, 2018 at 11:38

1 Answer 1


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: log of the full posterior

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:

full posterior with linearized constraints

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$.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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