IMU class debugged and works