%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 5.7 Unstable plant 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%PLANT (5.62)
omega=logspace(-2,2,501);
p = 1.0;  
g1 = nd2sys([1],[1 -p]); dyn = nd2sys(5,[10 1]); G = mmult(g1,dyn);
kd=0.5; Gd=nd2sys(kd,[0.2 1.2 1]);


% PID
Kc=0.4;
c1=nd2sys([10 1],[10 1.e-5],Kc); c2=nd2sys([10 1],[.1 1]); 
c3 = nd2sys(1,[.1 1]); c2=mmult(c2,c3);
K=mmult(c1,c2);

L = mmult(G,K); 

Gr = frsp(G,omega); Kr = frsp(K,omega); Lr=frsp(L,omega); Gdr=frsp(Gd,omega);

offsy = 0.5;
l1 = [3e-2 p; 1 p; 2 Inf];

subplot(221);
vplot('liv,lm',Gr,Gdr,1,':', l1, ':');
axis([1e-2 20 1e-2 1e1])
xlabel('Frequency');
ylabel('Magnitude')
text(p,2e-2,'p');
text(.35,2,'G');
text(3,.2,'Gd');

[Ac,Bc,Cc,Dc]=unpck(K);
[Ag,Bg,Cg,Dg]=unpck(G);
[Ad,Bd,Cd,Dd]=unpck(Gd);

Expl5_7s;    %display block diagram
[t,x,y]=rk45('Expl5_7s', 10, [], [1e-6, 0.002 0.02]);

u1 = vpck(uout,tout);  y1=vpck(yout,tout);     % WITH CONSTRAINTS
u2 = vpck(u2out,tout); y2=vpck(y2out,tout);  

% Data from simulink
u1 = vpck(uout,tout); y1=vpck(yout,tout);
u2 = vpck(u2out,tout); y2=vpck(y2out,tout);  % WITH CONSTRAINTS

subplot(222);
vplot(y1,u1,y2,'--',u2,'--',0,':');
axis([0,10,-1.5,1.5]);
xlabel('Time');
text(5,-0.7,'y');
text(6,1.15,'u');


