Diagonalize Matrix