Middle of fixing IMU

Not finished