Fix matrix diagonals calculation.