%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This program implements the indirect neural controller
% for the surge tank example.
%
% Kevin Passino
% Version: 3/25/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);

% Define the parameters for the gradient adaptation method

eta=1.25;
gamma=1;
Walpha=0.01;
Wbeta=0.01;
epsilon(1)=Walpha; % Just pick this to be something reasonable

% Define parameters of the approximators

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

theta(:,1)=ones(2*nR,1);
thetabeta(:,1)=(beta0+0.5*(beta1-beta0))*theta(nR+1:2*nR,1);  % Set initial thetabeta value
      													% (the middle of the range)
thetaalpha(:,1)=0*theta(1:nR,1);      % Set initial thetaalpha value (not a good guess)

paramerrornorm(1)=0;  

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

c(1,1)=0;  % Next, initialize the centers - just make them uniform
sigma=0.2;

% Next, form the phih vector

for i=2:nR
	c(1,i)=c(1,i-1)+0.2;
end

for i=1:nR
	phih(i,1)=exp(-(h(1)-c(1,i))^2/sigma^2);
end

% Next, find the initial estimates of the plant dynamics
	
betahat(1)=thetabeta(:,1)'*phih(:,1);
alphahat(1)=thetaalpha(:,1)'*phih(:,1);

% Next, define the intial controller output
	
u(1)=(1/(betahat(1)))*(-alphahat(1)+r(1+1));

% Next, form the phi vector

phi(:,1)=[phih(:,1)', u(1)*phih(:,1)']';
	
% Initialize estimate of the plant dynamics (note that the 
% time indices are not properly aligned but this is just an estimate
	
hhat(1)=alphahat(1)+betahat(1)*u(1); % Estimate to be the same as at 
									 % the first time step

									 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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)=Walpha+Wbeta*abs(u(k-1));
	
	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:
	
	theta(:,k)=theta(:,k-1)-...
	(eta*phi(:,k-1)*eepsilon(k))/(1+gamma*phi(:,k-1)'*phi(:,k-1));
	
	thetabeta(:,k)=theta(nR+1:2*nR,k);  
	thetaalpha(:,k)=theta(1:nR,k);  
	
	% Perform projection for betahat(k) so that the control input
	% is well-defined
	
	for i=1:nR
		if thetabeta(i,k)<beta0
			thetabeta(i,k)=beta0;
		end
	end

	% Next, fix the theta vector with what the projection suggested
	% (of course we do not have to fix the thetaalpha part since we 
	% do not use projection for it).
	
	theta(nR+1:2*nR,k)=thetabeta(:,k); 
	
	% Next, for plotting, compute the norm of the parameter error
	
	paramerrornorm(k)=((theta(:,k)-theta(:,k-1))'*(theta(:,k)-theta(:,k-1)));
	
	% Next, compute the phih vector:
	
	for i=1:nR
		phih(i,k)=exp(-(h(k)-c(1,i))^2/sigma^2);
	end

	% Next, find the estimates of the plant dynamics
	
	betahat(k)=thetabeta(:,k)'*phih(:,k);
	alphahat(k)=thetaalpha(:,k)'*phih(:,k);

	% Store the estimate of the plant dynamics
	
	hhat(k)=alphahat(k-1)+betahat(k-1)*u(k-1);
	
	% Next, use the certainty equivalence controller
	
	u(k)=(1/(betahat(k)))*(-alphahat(k)+r(k+1));
	
	% Form the phi vector
	
	phi(:,k)=[phih(:,k)', u(k)*phih(:,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);
	
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-')
grid
title('Tank input, u')
xlabel('Time, k')
axis([min(time) max(time) -50 50])


%%%%%%%%
figure(2)
clf
subplot(311)
plot(time,h,'k-',time,hhat,'k--')
grid
title('Liquid level h and estimate of h')

subplot(312)
plot(time,alpha,'k-',time,alphahat,'k--')
grid
title('Plant nonlinearity \alpha and its estimate')

subplot(313)
plot(time,beta,'k-',time,betahat,'k--')
grid
xlabel('Time, k')
title('Plant nonlinearity \beta and its estimate')

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


%%%%%%%%%%%%%%
figure(4)
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
% nonlinearity in the plant (i.e., at time k)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

height=0:0.05:10;

for j=1:length(height)
	
	% Estimate of the nonlinearity (at time k)
	
	for i=1:nR
		phiht(i,j)=exp(-(height(j)-c(1,i))^2/sigma^2);
	end

	alphahatt(j)=thetaalpha(:,k)'*phiht(:,j);
	betahatt(j)=thetabeta(:,k)'*phiht(:,j);

	% Actual nonlinearity

	At(j)=abs(abar*height(j)+bbar);

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

%%%%%%%%%%%%%
figure(5)
clf
subplot(311)
plot(height,alphat,'k-',height,alphahatt,'k--')
grid
title('\alpha and its estimate at the last time step')

subplot(312)
plot(height,betat,'k-',height,betahatt,'k--')
grid
title('\beta and its estimate at the last time step')
axis([min(height) max(height) 0 1])

subplot(313)
plot(height,phiht,'k-')
grid
title('\phi_h activation functions')
xlabel('Liquid height, h')

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