Added slam functions and unit-tests