Generalization and fixes to CleanUpMatrix