Added quaternion math for invert, calculating state error difference