EKF algorightm porting done