EKF implemetantion extracted from SLAM