Orthonormalised the rotation matrix

Added a naive orthonormalisation function and applied it to the rotation matrix implementation.