MADE:
--> Solved trouble with ground tracking EKF. Tested and seems to work rightly.