Fix for diagonalization with elimination of constraints