Robots and rovers exploring planets need to autonomously navigate to specified locations. Advanced Scientific Concepts, Inc. (ASC) and the University of Minnesota will develop a navigational system that employs an IMU and a 3D FLASH Lidar camera manufactured at ASC. The system will furnish both the position of the rover and an elevation map of the terrain. The map will be useful in detecting hazards to navigation both by rovers and during entry, descent and landing (EDL). The algorithm is designed to function in real-time with the comparatively slow speed computers available in space by employing an advanced algorithm that makes efficient use of Lidar determined landmarks. Those landmarks that only appear in a few images are not retained in the state vector, but nevertheless furnish constraints on the rover's pose for improving its state estimates. Those landmarks that persist in many images are used for improving the accuracy of both the rover's state and the map at complexity only linear in the number of landmarks. Significant components of this approach have enjoyed success in NASA tests in the Mohave Desert for EDL and have been assessed at level TRL 4.