Step 1 of EKF SLAM as described in SLAM for dummies should be finished now