First implementation of Graph slam. class is messy a bit needs work to clean up. Based on udacity AI course.
(fixes issueID #1 @4.50h )