Changed imu algorithm