Adding diagonal matrix.