Fairly big commit.  MadgwickAHRS has been tailored for BoneMarrow use inside of IMU.h/.c  invSqrt has been moved.  Currently, the diagnostic executable is performing a single-frame test.

Signed-off-by: Anthony Clay <anthony.clay@zarthcode.com>