%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This program implements the indirect genetic 
% adaptive controller for the surge tank example.
%
% Kevin Passino
% Version: 4/30/99, fixed 10/22/03 with fix to ga.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize the GA parameters:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rand('state',0) % Reset the random number generator so each time you re-run
                % the program with no changes you will get the same results.

  NUM_TRAITS=2;          % Number of traits in each individual (tune two parameters)
  HIGHTRAIT=[6 beta1];   % Upper limit of a trait
  LOWTRAIT=[-2 beta0]; 	 % Lower limit of a trait
  SIG_FIGS=[5 5]';       % Number of genes in each trait
  DECIMAL=[1 1];         % Order of magnitude the trait (due to known bounds)
  MUTAT_PROB=0.05;       % Probability of mutation (typically <.1)
  CROSS_PROB=0.9;        % Probability of crossover (typically near 1)
  POP_SIZE=10;           % Number of individuals in the population
  ELITISM=1;             % Elitism ON/OFF, 1/0


% Define the initial guess at the plant dynamics (make each member of the 
% population the same initial guess)

% Define initial parameter estimates:

thetaalpha(1)=2;
thetabeta(1)=0.5;

% Then define the whole population to have the same values:

for pop_member=1:POP_SIZE
    trait(1,pop_member,1)=thetaalpha(1);
    trait(2,pop_member,1)=thetabeta(1);
end

% Make the initial estimates of the plant dynamics based on this:
	
alphahat(1)=thetaalpha(1)*h(1);
betahat(1)=thetabeta(1);

% Number of steps to look back in assessment of quality of identifier model:

N=100;

gamma=0.001;  % Used in forming the fitness function from Js

% Next, define the intial controller output
	
u(1)=(1/(betahat(1)))*(-alphahat(1)+r(1+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, we need to set some values based on the values input by 
% the user.  In particular, we determine the length of the 
% chromosome and start point of each trait.  This information 
% will be used later in the main algorithm.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   CHROM_LENGTH=sum(SIG_FIGS)+NUM_TRAITS; % Length of the chromosome is 
  										  % the number of sig. figs. plus
  										  % the number of sign positions
   TRAIT_START(1)=1;					  % Initialize: the first trait 
   										  % starts at the first digit 
   										  % (this is the sign digit)

   for current_trait=1:NUM_TRAITS, 	% Determine the start point of the
   									% other traits - it is the start of
   									% the last trait plus the no. of sig. 
   									% figs. plus one for sign
TRAIT_START(current_trait+1)=...
   TRAIT_START(current_trait)+SIG_FIGS(current_trait)+1;
   % Yes, we compute the TRAIT_START for one extra trait - this 
   % is used for convenience in the code below.
   end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
	
	e(k)=r(k)-h(k);  % For plotting, if needed

% Define the genetic adaptive controller:
	
if k>N+1, 
	
	% Next, we have to check how each member of the population would have
	% done over the last several steps if it had been used as the identifier
	% The objective is to compute Js which will be used in the fitness function
	
	for pop_member=1:POP_SIZE
		% Initialize:
	    Js(pop_member,k)=0;
	    for j=k-N:k,
			hhats(pop_member,j)=trait(1,pop_member,k-1)*h(j-1)+trait(2,pop_member,k-1)*u(j-1);
		    es(pop_member,j)=hhats(pop_member,j)-h(j);
	        Js(pop_member,k)=Js(pop_member,k)+(es(pop_member,j))^2;
		end
		Jbar(pop_member,k)=1/(gamma+Js(pop_member,k));  % Computes the fitness function for each member
		sumfitness=sum(Jbar(:,k)); % Used also below in GA for selection
		Jbaravg(k)=sumfitness/POP_SIZE; % Find average fitness
	end
	
	% Pick the best member to be used to specify the estimate
	
	[value,bestmember(k)]=min(Js(:,k));
	
	bestindividual(:,k)=trait(:,bestmember(k),k-1);  % For plotting if needed
												  % and for use in GA elitism

	thetaalpha(k)=trait(1,bestmember(k),k-1);  
	thetabeta(k)=trait(2,bestmember(k),k-1);  
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% GA operations to produce the next genertion (i.e, generation at k)
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
% This part assumes that the traits at time k-1 are in range to start with

% First, convert the traits to the chromosome form for use with the genetic operators.

for pop_member = 1:POP_SIZE

for current_trait = 1:NUM_TRAITS,

% First, we transfer the sign of the trait into the chromosome

               if trait(current_trait,pop_member,k-1) < 0
                  pop(TRAIT_START(current_trait),pop_member)=0;
               else
                  pop(TRAIT_START(current_trait),pop_member)=9;
               end

% Next, strip off the sign and store the resulting value in a
% temporary variable that is used in the construction of pop

temp_trait(current_trait,pop_member)=...
           abs(trait(current_trait,pop_member,k-1));  
		% temp_trait is trait without the sign of trait 

% Next, we store the numbers of the trait in the chromosome:
% First, set up a temporary trait with at most
% one nonzero digit to the left of the decimal point.
% This is used to strip off the numbers to put 
% them into a chromosome.

temp_trait(current_trait,pop_member)=...
temp_trait(current_trait,pop_member)/10^(DECIMAL(current_trait)-1);

% Encode the new trait into chromosome form 

for make_gene = TRAIT_START(current_trait)+1:TRAIT_START(current_trait+1)-1,
               
% For each gene on the trait make the gene the corresponding digit on 
% temp_trait (note that rem(x,y)=x-roundtowardszero(x/y)*y or 
% rem(x,1)=x-roundtowardszero(x) so that rem(x,1) is the fraction part
% and x-rem(x,1) is the integer part so the next line makes the location in
% pop the same as the digit to the left of the decimal point of temp_trait
  
pop(make_gene,pop_member)=temp_trait(current_trait,pop_member)-...
   rem(temp_trait(current_trait,pop_member),1);

% Next, we take temp_trait and rotate the next digit to the left so that
% next time around the loop it will pull that digit into the 
% chromosome.  To do this we strip off the leading digit then shift 
% in the next one.
  
temp_trait(current_trait,pop_member)=...
  (temp_trait(current_trait,pop_member)-pop(make_gene,pop_member))*10;
               
end

         end % Ends "for current_trait=..." loop
         
	 end    % Ends "for pop_member=..." loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Apply genetic operators:
%
% First, form the mating pool.  
% To do this we select as parents the
% chromosomes that are most fit.  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   for pop_member = 1:POP_SIZE,
	if ELITISM ==1 & pop_member==bestmember(k)  % If elitism on, and have
											 % the elite member
		parent_chrom(:,pop_member)=pop(:,pop_member); % Makes sure that
		                          % the elite member gets into the next
								  % generation.
	else
      pointer=rand*sumfitness; % This makes the pointer for the roulette 
                               % wheel.  
      member_count=1;        % Initialization
      total=Jbar(1,k);

      while total < pointer,                % This spins the wheel to the 
       									    % pointer and finds the 
       									    % chromosome there - which is
       									    % identified by member_count
         member_count=member_count+1;
         total=total+Jbar(member_count,k);
      end
      
% Next, make the parent chromosome

    parent_chrom(:,pop_member)=pop(:,member_count);
	end
   end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reproduce section (i.e., make off-spring - "children")
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% In the approach below each individual gets to mate and
% they randomly pick someone else (not themselves) in the 
% mating pool to mate with.  Resulting children are 
% composed of a combination of genetic material of their
% parents.  If elitism is on, when the elite member gets
% a chance to mate they do not; they are simply copied
% over to the next generation.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for parent_number1 = 1:POP_SIZE,    % Crossover (parent_number1 is the 
									% individual who gets to mate
	if ELITISM ==1 & parent_number1==bestmember(k) % If elitism on, and 
											 % have the elite member
		child(:,parent_number1)=parent_chrom(:,parent_number1);
	else
	   parent_number2=parent_number1;  % Initialize who the mate is
	   while parent_number2 == parent_number1   % Iterate until find 
		                                        % a mate other than
												% yourself
         parent_number2 = rand*POP_SIZE; % Choose parent number 2
		 								 % randomly (a random mate)
         parent_number2 = parent_number2-rem(parent_number2,1)+1;     
	   end
    if CROSS_PROB > rand           % If true then crossover occurs

         site = rand*CHROM_LENGTH;   % Choose site for crossover
         site = site-rem(site,1)+1;  % and make it a valid integer 
         							 % number for a site

% The next two lines form the child by the swapping of genetic
% material between the parents

child(1:site,parent_number1)=parent_chrom(1:site,parent_number1);

child(site+1:CHROM_LENGTH,parent_number1)=...
     parent_chrom(site+1:CHROM_LENGTH,parent_number2);

      else                           % No crossover occurs

% Copy non-crossovered chromosomes into next generation
% In this case we simply take one parent and make them
% the child.

child(:,parent_number1)=parent_chrom(:,parent_number1);

	 end
   end  % End the "if ELITISM..." statement
end  % End "for parent_number1=..." loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Mutate children.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Here, we mutate to a different allele
% with a probability MUTAT_PROB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   for pop_member= 1:POP_SIZE,

	if ELITISM ==1 & pop_member==bestmember(k)  % If elitism on, and 
											 % have the elite member
		child(:,pop_member)=child(:,pop_member); % Do not mutate
											% the elite member
	else
	  for site = 1:CHROM_LENGTH,

         if MUTAT_PROB > rand        % If true then mutate  
            rand_gene=rand*10; 		 % Creat a random gene
            
            % If it is the same as the one already there then 
            % generate another random allele in the alphabet
            
            while child(site,pop_member) == rand_gene-rem(rand_gene,1),
               rand_gene=rand*10;
            end;
            
            % If it is not the same one, then mutate
            
            child(site,pop_member)=rand_gene-rem(rand_gene,1);
            
            % If takes a value of 10 (which it cannot
            % mutate to) then try again (this is a very low probability
			% event (most random number generators generate numbers
			% on the *closed* interval [0,1] and this is why this line
			% is included).
            
            if rand_gene == 10
                 site+site-1;
            end
         end  % End "if MUTAT_PROB > rand ... 
	   end  % End for site... loop
	 end  % End "if ELITISM..."
   end  % End for pop_member loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create the next generation 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   pop=child;               % Create next generation (children 
   							% become parents)
							
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, we have to convert the population (pop) to the base-10 
% representation (called trait) so that we can check if the traits
% all still lie in the proper ranges specified by HIGHTRAIT and LOWTRAIT
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for pop_member = 1:POP_SIZE

	for current_trait = 1:NUM_TRAITS,
			 
trait(current_trait,pop_member,k)=0; % Initialize variables
place_pointer=1;

% Change each of the coded traits on the chromosomes into base-10 traits: 
% For each gene on the current_trait past the sign digit but before the
% next trait find its real number amount and hence after finishing
% the next loop trait(current_trait,pop_member,k) will be the base-10
% number representing the trait

for gene=TRAIT_START(current_trait)+1:TRAIT_START(current_trait+1)-1,
 place=DECIMAL(current_trait)-place_pointer;
 trait(current_trait,pop_member,k)=...
 trait(current_trait,pop_member,k)+...
    (pop(gene,pop_member))*10^place;
 place_pointer=place_pointer+1;
end

% Determine sign of the traits and fix 
% trait(current_trait,pop_member,k) so that it has the right sign:

if pop(TRAIT_START(current_trait),pop_member) < 5
   trait(current_trait,pop_member,k)=...
      -trait(current_trait,pop_member,k);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fix bad traits (i.e., ones that are out of the range 
% specified by HIGHTRAIT and LOWTRAIT) by saturation at the extremes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if trait(current_trait,pop_member,k)>HIGHTRAIT(current_trait)

                     % The trait has went higher than the upper
                     % bound so let the trait equal to the
                     % HIGHTRAIT bound.

trait(current_trait,pop_member,k)=HIGHTRAIT(current_trait);

% Now consider the other case:

elseif trait(current_trait,pop_member,k)<LOWTRAIT(current_trait)

                     % The trait has went lower than the lower 
                     % bound so let the trait equal to the
                     % LOWTRAIT bound

trait(current_trait,pop_member,k)=LOWTRAIT(current_trait);
                  
		end


	end % Ends "for current_trait=..." loop
         
end    % Ends "for pop_member=..." loop
 
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% End main GA operations
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
else  % Hold estimates constant if have not gotten enough data
	  % to be able to compute the fitness function (hence, adaptation
	  % does not start until after N steps)

	trait(:,:,k)=trait(:,:,k-1);
	
	% Pick the parameters to be the same (here, just pick it to be the first
	% population member - since they are all the same initially it does not
	% matter which one you pick)
	
	thetaalpha(k)=trait(1,1,k-1);  
	thetabeta(k)=trait(2,1,k-1);  
	
end

	% Next, find the estimates of the plant dynamics (elite member)
	
	betahat(k)=thetabeta(k);
	alphahat(k)=thetaalpha(k)*h(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));
		
	% 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
subplot(211)
plot(time,Jbaravg,'k-')
grid
title('Average fitness')

subplot(212)
plot(time,bestmember,'k-')
grid
xlabel('Time, k')
T=num2str(POP_SIZE);
T=strcat('Index of best member in population of size=',T);
title(T)



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