dens_ini
[num1,den]=ss2tf(A,B,C,D,1);
[num2,den]=ss2tf(A,B,C,D,2);
[num3,den]=ss2tf(A,B,C,D,3);
[num4,den]=ss2tf(A,B,C,D,4);
delay=0;
delta=0;
g11=poly2tfd(num1(1,:),den,delta,delay);
g21=poly2tfd(num1(2,:),den,delta,delay);
g12=poly2tfd(num3(1,:),den,delta,delay);
g22=poly2tfd(num3(2,:),den,delta,delay);
dg11=poly2tfd(num2(1,:),den,delta,delay);
dg21=poly2tfd(num2(2,:),den,delta,delay);
dg12=poly2tfd(num4(1,:),den,delta,delay);
dg22=poly2tfd(num4(2,:),den,delta,delay);
ts=0.4;
tend=10;
plant=tfd2step(tend,ts,[1 1],g11,g21,g12,g22);
dplant=tfd2step(tend,ts,[1 1],dg11,dg21,dg12,dg22);
plotstep(plant);
plotstep(dplant);
model=plant;
M=3;
P=10;
tf=20;
ywt=[1 1];
uwt=[0.01 0.01];
r=[1 1];
d=[-1 1];
kmpc=mpccon(model,ywt,uwt,M,P);
[y,u]=mpcsim(plant,model,kmpc,tf,r,[],[],dplant,[],d);
plotall(y,u,ts);
