%Example 4. 5 Controllability of tanks in serise.
%
%Original file from:
%------------------------------------------------------------------------------
%
% Simexample.m
%
% Roy Smith:	very early AM, Thurs, Oct 6th, 1994.
%
% Implement Sigurd's suggested four tank simulation to show that being state
%   controllable doesn't get you everything.
%
%------------------------------------------------------------------------------

%	Tanks are modeled by a single lag, with unity gain, and are cascaded.
%	The input flow is constant, and we vary only the input temperature.

tc1 = 0.01;		% 100 seconds time constant.
tc2 = 0.01;
tc3 = 0.01;
tc4 = 0.01;

%	The state = [t1 t2 t3 t4]'

A = [-tc1,    0,   0,     0; ...
      tc2, -tc2,   0,     0; ...
        0,  tc3, -tc3,    0; ...
        0,    0,  tc4, -tc4];

B = [tc1; 0; 0; 0];

C = eye(4,4);

fourtanks = pck(A,B,C);

%	Check the controllability of the system.

Co = ctrb(A,B);
rCo = rank(Co);
disp(['rank controllability matrix = ' int2str(rCo)])

% Now specify the desired response after h seconds.  The initial condition is
% zero. If h is set the large the numerical errors coming from the simulation
% of an unstable system swamp us.

% We allow negative temperatures as we assume that this has been 
% linearized about some point.

h = 400;				% final time
xh = [1; -1; 1; -1];	% desired temperatures

eA = expm(-A*h);
Q = eA*B*B'*eA' - B*B';
X = lyap(A,Q);

% calculate the required input, uspec, by simulating the adjoint, from the
% final state as the initial condition.

Cadj = B';
Aadj = -A';
Badj = ones(4,1);		% dummy B.  input will be zero anyway.
xadj0 = inv(X)*eA*xh;

Uadj = pck(Aadj,Badj,Cadj);

%	make it go to zero after the desired input.

del=0.5;
uspec = trsp(Uadj,0,h,del,xadj0);
uspec = tackon(uspec,vpck(0,h+del));
yspec = trsp(fourtanks,uspec,1.25*h,del);

subplot(211),vplot(uspec,0,':',[0 400; 0 500; 2 Inf]);
xlabel('TIME [seconds]'); ylabel('INPUT SIGNAl')
title(['INPUT TO GIVE DESIRED STATE AT  ' num2str(h) ' SECONDS'])
text(370,90,'T0');

subplot(212),vplot(yspec,vpck(xh,h),'*');
xlabel('TIME [seconds]'); ylabel('STATES')
title(['RESPONSE OF STATES (TANK TEMPERATURES)']);
text(240,16,'T1');text(250,5,'T2');text(290,0,'T3');text(300,-7,'T4');
axis([0 500 -20 20]);
