Fixed bug in EKF