Added slam functions
Added unit tests