Integrated EKF-SLAM implementation