Algorithms for Localization and Routing of Unmanned Vehicles in GPS-Denied Environments