Robot Arm IMU integration

The IMU control has been integrated into the robot arm control system.
All the math looks (fairly) good except when the IMUs roll.
There is a standing issue that a roll of any kind breaks the math (with
no clear solution proposed).