Orthonormalised the rotation matrix
Orthonormalised roll as well
Merge pull request #7 from jellymann/ds-orthonormalise

Orthonormalised the rotation matrix