MADE:
--> Finalized first version of EKF

TODO:
--> continue with communication