Concurrent Mapping and Localization