Added Step 2 of SLAM.