Added matrix diagonalization