Implemented all methods of ekf. Untested.
Adding file for testing of the ekf