started improving EKF stuff