Simple imu debug, and other fixes

Initially just contained a way to debug the state of the simple IMU fusion as @pblack suggested, but when I was extracting it from my current dev branch I realized there were a few other small things that could be merged easily too.