Simultaneous Localization, Control and Mapping