Fixed bug in EKF SLAM output