Fix some (not all) errors in ndiagonal_matrix code