fixed bugs in EKF and implemented constant velocity motion model filter