MADE:
--> StereoEKF seems to be right as groundEKF. Only tested with one dataset.

TODO
--> Test with other datasets