Updates to the bench testing suite.
Added the imu info message to output debugging imu data.