Fixed the diagonal matrix bug