Full 3D EKF-Based SLAM