MADE:
--> Created a test case to GroundTrackingEKF
--> Tested case and seems to be right.

TODO:
--> Test with EC-SAFEMOBIL and confirm the equations in order to use them in StereoEKF