Matrix D_lqr is not correctly constructed if integral action is used in 'refgain' form. The line:
D_lqr = np.hstack([Kf, -K])
should read:
D_lqr = np.hstack([Kf, -K[:, :sys_nstates]])
as K[:, sys_nstates:] is already used in C_lqr, D_lqr is too wide.