MADE:
--> Changed equations of the GroundTrackingEKF according coordinates to intuitive coordinates.

TODO:
--> Test modified functions
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
MADE:
--> Changes in groundEKF with Alex images and vicon data. Adapted to new GroundTrackingEKF interface
MADE:
--> Changed equations for StereoVisionEKF but not tested...
MADE:
--> Changed StereoVisionEKF and GroundTrackingEKF access to matrixes
--> Changed StereoVisionEKF test, is stable but result is wrong. I remember same translation with first changes in ground EKF, revise equiations
MADE:
--> Revised TestGroundEKF.cpp and TestSegmentation.cpp looking for the error. Not found.
--> Going to test with simulation (To "investigate" if error is in BOViL class or in the data set conversion