IMU System able to control simulated arms using quaternions and a fast inverse kinematics optimization

A lot of additions here that allow IMUs to be read with no gimbal lock, give accurate arm orientation and position estimates, then use that estimate to run through a (very greatly improved) inverse kinematics optimization. Simulation was added that allows a user to put on two IMUs and control the simulated robot arm in close to real time (slight ~0.15s lag), but is very accurate and works really well.