Misc. code to implement Simultaneous Localization and Mapping for Autonomous Robots