Jesper, touching up in IMU code