Fixed a bug in size estimation of diagonal matrices and wrote a test for it