%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This program implements a controller that uses a planning
% strategy for the surge tank example.
%
% Kevin Passino
% Version: 4/19/01
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialize variables

clear

% Set the length of the simulation

Nnc=300;

T=0.1;      % Sampling rate

% As a reference input, we use a square wave (define one extra 
% point since at the last time we need the reference value at
% the last time plus one)

timeref=1:Nnc;
r(timeref)=3.25-3*square((2*pi/150)*timeref); % A square wave input
%r(timeref)=3.25*ones(1,Nnc+1)-3*(2*rand(1,Nnc+1)-ones(1,Nnc+1)); % A noise input
ref=r(1:Nnc);  % Then, use this one for plotting

time=1:Nnc;
time=T*time; % Next, make the vector real time


% We assume that the parameters of the surge tank (truth model) are:

abar=0.01;  % Parameter characterizing tank shape (nominal value is 0.01)
%abar=0.05;  % Parameter characterizing tank shape 
bbar=0.2;   % Parameter characterizing tank shape (nominal value is 0.2)
cbar=1;     % Clogging factor representing dirty filter in pump (nominally, 1)
%cbar=0.8;     % Clogging factor representing dirty filter in pump (dirty case)
dbar=1;     % Related to diameter of output pipe 
g=9.8;      % Gravity

% Controller parameters

% Model used in planning strategy:

abarm=0.002;  % Parameter characterizing tank shape 
bbarm=0.2;   % Parameter characterizing tank shape 
cbarm=0.9;  % Clogging factor representing dirty filter in pump 
dbarm=0.8;   % Related to diameter of output pipe 

% Next, use a set of preset controllers (think of each as a plan template)
% In this case we use PI controllers, with a grid of Kp, Ki gains around the 
% ones that we had designed for the PI controller for the **model**

% Guess at values 
Kp=0.01;
Ki=0.3;

%Kpvec=Kp; % Vector of gains in the region of Kp, Ki
%Kivec=Ki;

Kpvec=0:0.05:0.2; % Vector of gains in the region of Kp, Ki
%Kivec=Ki;
Kivec=0.15:0.05:0.4;

%Kpvec=0.2:0.2:1.8; % Vector of gains in the region of Kp, Ki
%Kivec=0.05:0.01:0.11;

% Set weights of cost function used to select plans
w1=1;
w2=1;

% Set the length of time that will project into the future, N
%NN=[20]; % To test just one value for N
NN=[1,5,10,15,17,20,25,30,33,35,36,37,38,39,40,45,50,51,52,53,55,56,59,60,61,65,69,70];  % To test multiple values of the projection length
N=max(NN); % For initialization, and allocation of variables below


% First, compare the truth model characteristics to the model

height=0:.1:10;

for i=1:length(height)
crosssect_truth(i,1)=abs(abar*height(i)+bbar);
crosssect_model(i,1)=abarm*height(i)^2 +bbarm;
end

figure(1)
clf
plot(height,crosssect_truth,'k-',height,crosssect_model,'k--')
grid
ylabel('Cross-sectional area')
xlabel('Height')
title('Cross-sectional area for plant (solid) and model (dashed)')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, start the main loop:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for kk=1:length(NN)
	N=NN(kk); % Next projection length

	h=0*ones(1,Nnc+1);  % Allocate memory
	hp=0*ones(1,Nnc+1);
	hmm=0*ones(1,Nnc+1);
	h(1)=1;     % Initial liquid level 
	hp(1)=1;     % Initial liquid level  (for plant with a PI controller)
	hmm(1)=1;     % Initial liquid level  (for model with a PI controller)

	% Allocate memory for projection variables

	hm=0*ones(N+1,length(Kpvec),length(Kivec));
	em=0*ones(N,length(Kpvec),length(Kivec));
	um=0*ones(N,length(Kpvec),length(Kivec));

	J=0*ones(length(Kpvec),length(Kivec));
	rowindex=0*ones(1,Nnc);
	colindex=0*ones(1,Nnc);

	% Also, allocate memory for the control inputs and error

	up=0*ones(1,Nnc);
	umm=0*ones(1,Nnc);
	u=0*ones(1,Nnc);

	ep=0*ones(1,Nnc);
	emm=0*ones(1,Nnc);
	e=0*ones(1,Nnc);
	
	% Initialize controller integrator:

	sume=0;
	sumep=0;
	sumemm=0;

for k=1:Nnc

	%---------------------------------------------------------------
	% Define the controller (for plant), where test single Kp, Ki gains
	
	ep(k)=r(k)-hp(k);
	sumep=sumep+ep(k); % Compute integral approx.
	
	up(k)=Kp*ep(k)+Ki*sumep;
			
	% In implementation, the input flow is restricted 
	% by some physical contraints. So we have to put a
	% limit for the input flow that is chosen as 50.

    if up(k)>50
      up(k)=50;
    end
    if up(k)<-50
     up(k)=-50;
    end

	% Plant equations
	
	Ap=abs(abar*hp(k)+bbar); % Cross-sectional area

	alphap=hp(k)-T*dbar*sqrt(2*g*hp(k))/Ap; % Nonlinear dynamics
	betap=T*cbar/Ap;

	hp(k+1)=alphap+betap*up(k); % Compute plant output
	hp(k+1)=max([0.001,hp(k+1)]); % Constraint liquid not to go
							    % negative

	%---------------------------------------------------------------
	% Define the controller (for model), also where test single Kp, Ki
	% gains - here for the purpose of seeing how good the model is
	% (test model validity by seeing if it reacts similarly to the plant
	% for the same control system).
	
	emm(k)=r(k)-hmm(k);
	sumemm=sumemm+emm(k); % Compute integral approx.
	
	umm(k)=Kp*emm(k)+Ki*sumemm;
			
	% In implementation, the input flow is restricted 
	% by some physical contraints. So we have to put a
	% limit for the input flow that is chosen as 50.

    if umm(k)>50
      umm(k)=50;
    end
    if umm(k)<-50
     umm(k)=-50;
    end

	% Model equations
	
	Amm=abarm*hmm(k)^2 +bbarm; % Cross-sectional area (model)

	alphamm=hmm(k)-T*dbarm*sqrt(2*g*hmm(k))/Amm; % Nonlinear dynamics (model)
	betamm=T*cbarm/Amm;

	hmm(k+1)=alphamm+betamm*umm(k); % Compute plant output
	hmm(k+1)=max([0.001,hmm(k+1)]); % Constraint liquid not to go
							        % negative

	%---------------------------------------------------------------
	% Define the planner that uses the model to control the plant
	% (of course this is the primary thing that we want to study)
	
	e(k)=r(k)-h(k); % Compute error
	sume=sume+e(k); % Compute integral approx.
	

	% Projection into the future for each controller:
	
	hm(1,:,:)=h(k)*ones(length(Kpvec),length(Kivec)); % Initialize prediction with current
														% liquid level

	for ii=1:length(Kpvec)
		for jj=1:length(Kivec)  % The first two loops pick a controller
			% Initialize integral with current error integral sume, or 0 
			% (analgous to how we start the loop for the main controller)
			sumem=sume; 
			for j=1:N           % This loop simulates it at N time points

	em(j,ii,jj)=r(k)-hm(j,ii,jj); % Error for each model (assumes that r(k) will stay
									% constant over the projection window)
	sumem=sumem+em(j,ii,jj); % Compute integator for each controller
	um(j,ii,jj)=Kpvec(ii)*em(j,ii,jj)+Kivec(jj)*sumem; % Compute controller output
	
    if um(j,ii,jj)>50 % Compute saturation
      um(j,ii,jj)=50;
    end
    if um(j,ii,jj)<-50
     um(j,ii,jj)=-50;
    end

	Am=abarm*hm(j,ii,jj)^2 +bbarm; % Cross-sectional area (model)

	alpham=hm(j,ii,jj)-T*dbarm*sqrt(2*g*hm(j,ii,jj))/Am; % Nonlinear dynamics (model)
	betam=T*cbarm/Am;

	hm(j+1,ii,jj)=alpham+betam*um(j,ii,jj); % Compute plant output
	hm(j+1,ii,jj)=max([0.001,hm(j+1,ii,jj)]); % Constraint liquid not to go
							    				% negative
												
			end
		
	% Compute the cost function for the controller that was just simulated
	
	J(ii,jj)=w1*(r(k)*ones(N+1,1)-hm(1:N+1,ii,jj))'*(r(k)*ones(N+1,1)-hm(1:N+1,ii,jj))+...
	w2*um(1:N,ii,jj)'*um(1:N,ii,jj);
			
		end
	end
							
	% Find the indices of the best controller (plan)
	
	[val,rowindex(k)]=min(min(J')); % Gives the min value (val) and its row index
	[val,colindex(k)]=min(min(J)); % Gives the min value and its column index

	% Pick the input to be the one that came from the plan that did best in 
	% the projection
	
	u(k)=um(1,rowindex(k),colindex(k));
	
	% Next, put the chosen input to the plant
	
	% Input saturation

    if u(k)>50
      u(k)=50;
    end
    if u(k)<-50
     u(k)=-50;
    end

	% Plant equations
	
	A=abs(abar*h(k)+bbar); % Cross-sectional area

	alpha=h(k)-T*dbar*sqrt(2*g*h(k))/A; % Nonlinear dynamics
	beta=T*cbar/A;

	h(k+1)=alpha+beta*u(k); % Compute plant output
	h(k+1)=max([0.001,h(k+1)]); % Constraint liquid not to go
							    % negative

end

hh=h(timeref);

trackerrorenergy(kk)=0.5*(ref'-hh')'*(ref'-hh');
inputenergy(kk)=0.5*u*u';

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the results

% Plant: Controlled by PI controller 
% Strip off last computed value of h
hhp=hp(timeref);

figure(2)
clf
subplot(211)
plot(time,hhp,'k-',time,ref,'k--')
grid
ylabel('Liquid height, h')
title('Liquid level h and reference input r (plant)')

subplot(212)
plot(time,up,'k-')
grid
title('Tank input, u')
xlabel('Time, k')
axis([min(time) max(time) -50 50])

% Model: Controlled by PI controller
% Strip off last computed value of hmm
hhm=hmm(timeref);

figure(3)
clf
subplot(211)
plot(time,hhm,'k-',time,ref,'k--')
grid
ylabel('Liquid height, h_m')
title('Liquid level h_m and reference input r (model)')

subplot(212)
plot(time,umm,'k-')
grid
title('Tank input, u_m')
xlabel('Time, k')
axis([min(time) max(time) -50 50])

% Difference between plant and model: Controlled by PI controller
% Strip off last computed value of hmm

figure(4)
clf
subplot(211)
plot(time,hhp-hhm,'k-')
grid
title('Liquid height error')

subplot(212)
plot(time,up-umm,'k-')
grid
title('Tank input error')
xlabel('Time, k')


%-----------------------------------------------------------
% Planning system results
% Strip off last computed value of h
hh=h(timeref);

figure(5)
clf
subplot(211)
plot(time,hh,'k-',time,ref,'k--')
grid
ylabel('Liquid height, h')
title('Liquid level h and reference input r')

subplot(212)
plot(time,u,'k-')
grid
title('Tank input, u')
xlabel('Time, k')
axis([min(time) max(time) -50 50])

figure(6)
clf
plot(time,rowindex,'k-',time,colindex,'k--')
axis([min(time) max(time) 0 max(length(Kpvec),length(Kivec))])
grid
title('Indices of plan (row=solid, column=dashed)')
ylabel('Row and column indices')
xlabel('Time, k')

% Next, study the effect of the projection length N
figure(7)
clf
plot(NN,trackerrorenergy,'b-',NN,trackerrorenergy,'ro')
title('Tracking energy vs. projection length N')
xlabel('Projection length N')

figure(8)
clf
plot(NN,inputenergy,'b-',NN,inputenergy,'ro')
title('Control energy vs. projection length N')
xlabel('Projection length N')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%