Added slam functions and unit tests