Extracts from IMU work
These are fully-formed commits that can be merged without waiting for the rest of the branch to be done. A bit of a mix of random cleanups/fixes. Only the "fix initialization" commit actually changes behavior - we were initializing the simple fusion with the influence of gravity reversed/mirrored, meaning that it would always start up by slowly spinning (usually about x) to get to the right orientation.
All commits build.