MADE:
--> Rearranged operations in TestGroundEKF.cpp