Added orthonormalisation to the quat matrix as well.