Start implementation of EKF