Set position of imu_link accurately.
Better log messages for state transitions.