Add diagonal matrix solution