Add diagonal when creating sparse matrices