Continuing EKF slam...