clear all
s = tf('s'); 

theta = 1;

G = (0.01/(s+1.72e-4)/(4.32*s + 1))*[-34.54*(s+0.0572), 1.913; -30.22*s, -9.188*(s+6.95e-4)]*tf(1,1,'iodelay',theta);
G1 = pade(G,5);

tauc1 = 3*theta; tauc2 = theta;

% Diagonal Pairings

Kc1=-12.1/(tauc1+theta); tauI1=4*(tauc1+theta);
Kc2=-47.0/(tauc2+theta); tauI2=4.32; 

K1 = Kc1*(1 + 1/(tauI1*s)); K2 = Kc2*(1 + 1/(tauI2*s));

Cdiag = [K1 0;0 K2];

CLdiag = G1*Cdiag*inv(eye(2) + G1*Cdiag);
CLGddiag = inv(eye(2) + G1*Cdiag)*G1;

% off-diagonal pairings (the index refers to the output)
Kc1=52.3/(tauc1+theta+2.16); tauI1=4*(tauc1+theta + 2.16);
Kc2=-14.3/(tauc2+theta); tauI2=4.32;

K1 = Kc1*(1 + 1/(tauI1*s)); K2 = Kc2*(1 + 1/(tauI2*s));

G2 = G1*[0 1;1 0];

Coff = [K1 0;0 K2];

CLoff = G2*Coff*inv(eye(2) + G2*Coff);
CLGdoff = inv(eye(2) + G2*Coff)*G2;

time = 0:0.5:200;

r(1,1:length(time)) = 1;

r(2,1:length(time)/2) = 0;
r(2,length(time)/2+1:length(time)) = 1;

ydiag = lsim(CLdiag,r,time);
yoff = lsim(CLoff,r,time);

plot(time,ydiag)

figure, plot(time,yoff)


