dens_ini
ts=0.4;
[phi1,gam1]=c2dmp(A,B(:,[1 3]),ts);
[phi2,gam2]=c2dmp(A,B(:,[2 4]),ts);
mod=ss2mod(phi1,gam1,C,D(:,[1 3]),ts);
dmod=ss2mod(phi2,gam2,C,D(:,[2 4]),ts);
pmod=addumd(mod,dmod);
imod=pmod;
M=3;
P=10;
tf=20;
ywt=[1 1];
uwt=[0.01 0.01];
r=[1 1];
d=[-1 1];
ksmpc=smpccon(imod,ywt,uwt,M,P);
[y,u]=smpcsim(pmod,imod,ksmpc,tf,r,[],[],[],[],d,[]);
plotall(y,u,ts);
