Jesper, reverting changes made for code to Johans IMU