Simultaneous Planning Localization and Mapping