MADE:
--> Changed equations for StereoVisionEKF but not tested...