Updated SLAM algorithm for use with MrT dataset.