Fix bug in IMU