Make robust to slight non-orthogonality in rotation matrix
Make robust to slight non-orthogonality in rotation matrix

yasha0b1/robotics-toolbox-matlab,konradb3/robotics-toolbox-matlab,jaejunlee0538/robotics-toolbox-matlab,jaejunlee0538/robotics-toolbox-matlab,konradb3/robotics-toolbox-matlab,yasha0b1/robotics-toolbox-matlab,jaejunlee0538/robotics-toolbox-matlab,yasha0b1/robotics-toolbox-matlab,yasha0b1/robotics-toolbox-matlab,jaejunlee0538/robotics-toolbox-matlab,konradb3/robotics-toolbox-matlab,jaejunlee0538/robotics-toolbox-matlab,konradb3/robotics-toolbox-matlab,konradb3/robotics-toolbox-matlab,yasha0b1/robotics-toolbox-matlab