Advanced quaternion forward kinematics algorithm including overview of different methods for robot kinematics
robotics , vision and control - fundamental algorithms in matlab® .