Added Step 3 of SLAM.