%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This program implements the direct neural controller
% for the surge tank example.
%
% Kevin Passino
% Version: 4/12/99
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialize variables

clear

% We assume that the parameters of the tank are:

abar=0.01;  % Parameter characterizing tank shape (nominal value is 0.01)
bbar=0.2;   % Parameter characterizing tank shape (nominal value is 0.2)
cbar=1;     % Clogging factor representing dirty filter in pump (nominally, 1)
dbar=1;     % Related to diameter of output pipe 
g=9.8;      % Gravity
T=0.1;      % Sampling rate
beta0=0.25; % Set known lower bound on betahat
beta1=0.5;  % and the upper bound

% Set the length of the simulation

Nnc=1000;

% 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+1;
%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

% Next, set plant initial conditions

h(1)=1;          % Initial liquid level
e(1)=r(1)-h(1);  % Initial error

A(1)=abs(abar*h(1)+bbar);

alpha(1)=h(1)-T*dbar*sqrt(2*g*h(1))/A(1);
beta(1)=T*cbar/A(1);

% For the direct adaptive control case:
% Note that want etadirect<=2*gammadirect/beta1 (hence if you keep gammadirect=1
% you can raise etadirect to 4)
etadirect=1.25;
gammadirect=1;
Wu=0.01;
Nu=0;
epsilondirect=beta1*Wu+Nu;
epsilon(1)=epsilondirect;

% Define parameters of the approximator

nR=100;  % The number of receptive field units in the RBF

thetau(:,1)=0*ones(nR,1); % Note that there are only nR "stregths" to adjust

paramerrornorm(1)=0;  

n=2; % The number of inputs (since 1, use c(1,..) below)

tempc1=0:1:9;  % Defines a uniformly spaced vector roughly on the input domain
			   % that is used to form the uniform grid on the (h,r) space
			   % Note that 0.001 <= h <= 10 and 0.1 <= r <= 8  (notice that with this
			   % choice there is of course some overlap on the outer regions.
tempc2=0:1:9;

sigma=1; % Use the same value for all

% Next, form the phiu vector

k=0; % Counter for centers below

for i=1:10
	for j=1:10
	  k=k+1;
	  c(1,k)=tempc1(i);
	  c(2,k)=tempc2(j);
	end
end

for i=1:nR
	phiu(i,1)=exp(-([h(1),r(1+1)]'-c(:,i))'*([h(1),r(1+1)]'-c(:,i))/sigma^2);
end

% Next, compute the controller input

u(1)=thetau(:,1)'*phiu(:,1);

% For plotting purposes we also generate the "ideal" feedback linearizing
% controller output
ustar(1)=(1/beta(1))*(-alpha(1)+r(1+1));

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

for k=2:Nnc
	
% Define the plant
	
	% 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 u(k-1)>50
      u(k-1)=50;
    end
    if u(k-1)<-50
     u(k-1)=-50;
    end

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

% Define the controller
	
	e(k)=r(k)-h(k);
		
	% Define the deadzone size and its output:
	
	epsilon(k)=epsilondirect;
	
	if e(k)>epsilon(k)
		eepsilon(k)=e(k)-epsilon(k);
	end
	if abs(e(k))<=epsilon(k)
		eepsilon(k)=0;
	end
	if e(k)<-epsilon(k)
		eepsilon(k)=e(k)+epsilon(k);
	end

	% Next, perform the parameter update with the normalized gradient method:
	
	thetau(:,k)=thetau(:,k-1)+...
	(etadirect*phiu(:,k-1)*eepsilon(k))/(1+gammadirect*phiu(:,k-1)'*phiu(:,k-1));
	
	% Next, for plotting, compute the norm of the parameter error
	
	paramerrornorm(k)=((thetau(:,k)-thetau(:,k-1))'*(thetau(:,k)-thetau(:,k-1)));
	
	% Next, compute the phiu vector:
	
	for i=1:nR
		phiu(i,k)=exp(-([h(k),r(k+1)]'-c(:,i))'*([h(k),r(k+1)]'-c(:,i))/sigma^2);
	end

	% Next, compute the controller input

	u(k)=thetau(:,k)'*phiu(:,k);

	
	% Define some parameters to be used in the plant
	
	A(k)=abs(abar*h(k)+bbar);

	alpha(k)=h(k)-T*dbar*sqrt(2*g*h(k))/A(k);
	beta(k)=T*cbar/A(k);
	
	% Compute the ideal controller for plotting purposes
	
	ustar(k)=(1/beta(k))*(-alpha(k)+r(k+1));
	
end


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

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

subplot(212)
plot(time,u,'k-',time,ustar,'k--')
grid
title('Tank input, u and the "ideal" feedback linearizing input')
xlabel('Time, k')
axis([min(time) max(time) -50 50])


%%%%%%%%%%%%%%
figure(2)
clf
plot(time,paramerrornorm,'k-')
grid
xlabel('Time, k')
title('Norm of parameter error')


%%%%%%%%%%%%%%
figure(3)
clf

plot(time,e,'k-')
hold on
errorbar(time,0*ones(1,length(epsilon)),epsilon,'c-')
% Due to the samping period size the above will just make the deadzone
% area be a cyan shaded region.
grid
xlabel('Time, k')
title('Tracking error, e, and deadzone width')
axis([min(time) max(time) min([min(e),min(-epsilon)]) max([max(e), max(epsilon)])])



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, compute and display the final approximator mapping and
% ideal controller nonlinearity 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Request input from the user to see if they want to see the tuned
% controller mapping:

flag1=input('Do you want to see the nonlinear \n mapping implemented by the tuned neural \n controller? \n (type 1 for yes and 0 for no) ');

if flag1==1,
	
height=0:0.1:10;
reference=0:0.1:10;

for jj=1:length(height)

	% Define some parameters to be used in the plant
	
	At(jj)=abs(abar*height(jj)+bbar);

	alphat(jj)=height(jj)-T*dbar*sqrt(2*g*height(jj))/At(jj);
	betat(jj)=T*cbar/At(jj);

	for ii=1:length(reference)
				
	  for i=1:nR
		phiut(i)=exp(-([height(jj),reference(ii)]'-c(:,i))'*([height(jj),reference(ii)]'-c(:,i))/sigma^2);
	  end

	% Next, compute the controller input

	  ut(ii,jj)=thetau(:,k)'*phiut';
	
	% Compute the ideal controller for plotting purposes
	
	  ustart(ii,jj)=(1/betat(jj))*(-alphat(jj)+reference(ii));
	
	end
end



% Plot the data

figure(4)
clf
surf(height,reference,ut);
view(135,30);
colormap(white);
xlabel('Liquid height, h');
ylabel('Reference input, r');
zlabel('Controller output');
title('Direct neural controller mapping between inputs and output');


figure(5)
clf
surf(height,reference,ustart);
view(135,30);
colormap(white);
xlabel('Liquid height, h');
ylabel('Reference input, r');
zlabel('Ideal controller output');
title('Ideal controller mapping between inputs and output');

end


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