%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Radial Basis Function Neural Network for Tanker Ship Heading Regulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% By: Kevin Passino 
% Version: 1/12/00
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear		% Clear all variables in memory
pause off

% Initialize ship parameters 
% (can test two conditions, "ballast" or "full"):

ell=350;			% Length of the ship (in meters)
u=5;				% Nominal speed (in meters/sec)
%u=3;               % A lower speed where the ship is more difficult to control
abar=1;             % Parameters for nonlinearity
bbar=1;

% The parameters for the tanker under "ballast" conditions 
% (a heavy ship) are:

K_0=5.88;
tau_10=-16.91;
tau_20=0.45;
tau_30=1.43;

% The parameters for the tanker under "full" conditions (a ship
% that weighs less than one under "ballast" conditions) are:

%K_0=0.83;
%tau_10=-2.88;
%tau_20=0.38;
%tau_30=1.07;

% Some other plant parameters are:

K=K_0*(u/ell);
tau_1=tau_10*(ell/u);
tau_2=tau_20*(ell/u);
tau_3=tau_30*(ell/u);

% Parameters for the radial basis function neural network

% Define parameters of the approximator

nG=11;   % The number of partitions on each edge of the grid
nR=nG^2;  % The number of receptive field units in the RBF

n=2; % The number of inputs 

tempe=(-pi/2):(pi)/(nG-1):pi/2;  % Defines a uniformly spaced vector roughly on the input domain
			             % that is used to form the uniform grid on the (e,c) space
tempc=(-0.01):(0.02)/(nG-1):0.01;

k=0; % Counter for centers below

% Place the centers on a grid

for i=1:length(tempe)
	for j=1:length(tempc)
	  k=k+1;
	  center(1,k)=tempe(i);
	  center(2,k)=tempc(j);
	end
end

% Plot the center points of the grid

% Convert to degrees:

centerd=center*(180/pi);

figure(1)
clf
plot(centerd(1,:),centerd(2,:),'ko')
grid on
xlabel('Error e (deg.)')
ylabel('Change in error, c (deg./sec.)')
title('Grid of receptive field unit centers (each "o" is a center)')
axis([-110 110 -.8 .8])
hold on
istar=61; % Fix a special point where you will plot a RBF - and designate its center here
% Plot an dark o over the center point of the middle RBF, and some of its neighbors
neighbors=plot(centerd(1,istar),centerd(2,istar),'ko',centerd(1,istar+1),centerd(2,istar+1),'ko',centerd(1,istar+11),centerd(2,istar+11),'ko',centerd(1,istar+12),centerd(2,istar+12),'ko') 
set(neighbors,'LineWidth',2);
hold off

% Next, plot a radial basis function to show what it looks like - a Gaussian

% Define spreads of Gaussian functions

sigmae=0.7*((pi/nG)); % Use same value for all on e domain
sigmac=0.7*((0.02)/nG); 

% First, compute vectors with points over the whole range of 
% the neural controller inputs 

e_input=(-pi/2):(pi)/50:(pi/2); 
c_input=(-0.01):(0.02)/50:(0.01); 

% Next, compute the neural controller output for all these inputs

for jj=1:length(e_input) 
	for ii=1:length(c_input)
        % Pick the special RBFs
		rbfistar1(ii,jj)=2*exp(-(((e_input(jj)-center(1,istar))^2)/sigmae^2)-(((c_input(ii)-center(2,istar))^2)/sigmac^2));
		rbfistar2(ii,jj)=exp(-(((e_input(jj)-center(1,istar+11))^2)/sigmae^2)-(((c_input(ii)-center(2,istar+11))^2)/sigmac^2));
		rbfistar3(ii,jj)=2*exp(-(((e_input(jj)-center(1,istar+1))^2)/sigmae^2)-(((c_input(ii)-center(2,istar+1))^2)/sigmac^2));
		rbfistar4(ii,jj)=exp(-(((e_input(jj)-center(1,istar+12))^2)/sigmae^2)-(((c_input(ii)-center(2,istar+12))^2)/sigmac^2));
	end
end

% Convert from radians to degrees:

e_inputd=e_input*(180/pi);
c_inputd=c_input*(180/pi);

% Plot a receptive field unit (one that is not scaled)

figure(2)
clf
surf(e_inputd,c_inputd,rbfistar4);
view(145,30);
colormap(white);
xlabel('Heading error (e), deg.');
ylabel('Change in heading error (c), deg.');
zlabel('R_7_3(e,c)');
title('Receptive field unit R_7_3(e,c)');
rotate3d

% Next plot several receptive field units scalied and added together (RBF output)

figure(3)
clf
surf(e_inputd,c_inputd,rbfistar1+rbfistar2+rbfistar3+rbfistar4);
view(145,30);
colormap(white);
xlabel('Heading error (e), deg.');
ylabel('Change in heading error (c), deg.');
zlabel('Radial basis function neural network output');
title('Radial basis function neural network output, 2R_6_1(e,c)+R_6_2(e,c)+2R_7_2(e,c)+R_7_3(e,c)');
rotate3d
zoom

% Next, pick the strengths for the RBF 

temp=(-((nG-1)/2)):1:((nG-1)/2);

for i=1:length(temp) % Across the e dimension
	for j=1:length(temp) % Across the c dimension
	thetamat(i,j)=-((1/10)*(200*(pi/180))*temp(i)+(1/10)*(200*(pi/180))*temp(j));
	% Saturate it between max and min possible inputs to the plant
	thetamat(i,j)=max([-80*(pi/180), min([80*(pi/180), thetamat(i,j)])]);
						% Note that there are only nR "stregths" to adjust - here we choose them
	                    % according to this mathematical formula to get an appropriately shaped surface
	end
end

% And, put them in a vector

k=0; % Counter for centers below

for i=1:length(temp)
	for j=1:length(temp)
	  k=k+1;
	  theta(k,1)=thetamat(i,j);
	end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, provide a plot of the RBF neural controller surface:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


for jj=1:length(e_input) 
	for ii=1:length(c_input)
		
	for i=1:nR
		phit(i,1)=exp(-(((e_input(jj)-center(1,i))^2)/sigmae^2)-(((c_input(ii)-center(2,i))^2)/sigmac^2));
	end

	delta_output(ii,jj)=theta'*phit(:,1); % Performs summing and scaling of receptive field units

	end
end

% Plot the controller map

delta_output=delta_output*(180/pi);

figure(4)
clf
surf(e_inputd,c_inputd,delta_output);
view(145,30);
colormap(white);
xlabel('Heading error (e), deg.');
ylabel('Change in heading error (c), deg.');
zlabel('Controller output (\delta), deg.');
title('Radial basis function neural network controller mapping between inputs and output');
rotate3d
zoom


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the RBF regulating the ship heading 
	
% Next, we initialize the simulation:

t=0; 		% Reset time to zero
index=1;	% This is time's index (not time, its index).  
tstop=4000;	% Stopping time for the simulation (in seconds)
step=1;     % Integration step size
T=10;		% The controller is implemented in discrete time and
			% this is the sampling time for the controller.
			% Note that the integration step size and the sampling
			% time are not the same.  In this way we seek to simulate
			% the continuous time system via the Runge-Kutta method and
			% the discrete time controller as if it were
			% implemented by a digital computer.  Hence, we sample
			% the plant output every T seconds and at that time
			% output a new value of the controller output.
counter=10;	% This counter will be used to count the number of integration
			% steps that have been taken in the current sampling interval.
			% Set it to 10 to begin so that it will compute a controller
			% output at the first step.
			% For our example, when 10 integration steps have been
			% taken we will then we will sample the ship heading
			% and the reference heading and compute a new output
			% for the controller.  
eold=0;     % Initialize the past value of the error (for use
            % in computing the change of the error, c).  Notice
            % that this is somewhat of an arbitrary choice since 
            % there is no last time step.  The same problem is
            % encountered in implementation.  
x=[0;0;0];	% First, set the state to be a vector            
x(1)=0;		% Set the initial heading to be zero
x(2)=0;		% Set the initial heading rate to be zero.  
			% We would also like to set x(3) initially but this
			% must be done after we have computed the output
			% of the controller.  In this case, by
			% choosing the reference trajectory to be 
			% zero at the beginning and the other initial conditions
			% as they are, and the controller as designed,
			% we will know that the output of the controller
			% will start out at zero so we could have set 
			% x(3)=0 here.  To keep things more general, however, 
			% we set the intial condition immediately after 
			% we compute the first controller output in the 
			% loop below.


% Next, we start the simulation of the system.  This is the main 
% loop for the simulation of the control system.

while t <= tstop

% First, we define the reference input psi_r  (desired heading).

if t<100, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=100, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>2000, psi_r(index)=0; end      			% Then request heading of 0 deg
%if t>4000, psi_r(index)=45*(pi/180); end     % Then request heading of 45 deg
%if t>6000, psi_r(index)=0; end      			% Then request heading of 0 deg
%if t>8000, psi_r(index)=45*(pi/180); end     % Then request heading of 45 deg
%if t>10000, psi_r(index)=0; end      			% Then request heading of 0 deg
%if t>12000, psi_r(index)=45*(pi/180); end     % Then request heading of 45 deg

% Next, suppose that there is sensor noise for the heading sensor with that is
% additive, with a uniform distribution on [- 0.01,+0.01] deg.
%s(index)=0.01*(pi/180)*(2*rand-1);
s(index)=0;					  % This allows us to remove the noise.

psi(index)=x(1)+s(index);     % Heading of the ship (possibly with sensor noise).

if counter == 10,  % When the counter reaches 10 then execute the 
				   % controller

counter=0; 			% First, reset the counter

% Radial basis function neural network controller calculations:

e(index)=psi_r(index)-psi(index); % Computes error (first layer of perceptron)
c(index)=(e(index)-eold)/T; % Sets the value of c

eold=e(index);   % Save the past value of e for use in the above
				 % computation the next time around the loop

% Next, compute the RBF output

for i=1:nR
	phi(i,1)=exp(-(((e(index)-center(1,i))^2)/sigmae^2)-(((c(index)-center(2,i))^2)/sigmac^2));
end

	delta(index)=theta'*phi(:,1); % Performs summing and scaling of receptive field units

%
% A conventinal proportional controller:
%delta(index)=-e(index);

else % This goes with the "if" statement to check if the counter=10
     % so the next lines up to the next "end" statement are executed
     % whenever counter is not equal to 10

% Now, even though we do not compute the neural controller at each
% time instant, we do want to save the data at its inputs and output at
% each time instant for the sake of plotting it.  Hence, we need to 
% compute these here (note that we simply hold the values constant):

e(index)=e(index-1);	
c(index)=c(index-1); 
delta(index)=delta(index-1);

end % This is the end statement for the "if counter=10" statement

% Next, comes the plant:
% Now, for the first step, we set the initial condition for the
% third state x(3).

if t==0, x(3)=-(K*tau_3/(tau_1*tau_2))*delta(index); end

% Next, the Runge-Kutta equations are used to find the next state. 
% Clearly, it would be better to use a Matlab "function" for
% F (but here we do not, so we can have only one program).
  
	time(index)=t;

% First, we define a wind disturbance against the body of the ship
% that has the effect of pressing water against the rudder

%w(index)=0.5*(pi/180)*sin(2*pi*0.001*t);  % This is an additive sine disturbance to 
										% the rudder input.  It is of amplitude of
										% 0.5 deg. and its period is 1000sec.
%delta(index)=delta(index)+w(index);


% Next, implement the nonlinearity where the rudder angle is saturated
% at +-80 degrees

if delta(index) >= 80*(pi/180), delta(index)=80*(pi/180); end
if delta(index) <= -80*(pi/180), delta(index)=-80*(pi/180); end

% Next, we use the formulas to implement the Runge-Kutta method
% (note that here only an approximation to the method is implemented where
% we do not compute the function at multiple points in the integration step size).

F=[ x(2) ;
    x(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(x(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*x(2)^3 + bbar*x(2)) + (K/(tau_1*tau_2))*delta(index) ];
        
	k1=step*F;
	xnew=x+k1/2;

F=[ xnew(2) ;
    xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
   
	k2=step*F;
	xnew=x+k2/2;

F=[ xnew(2) ;
    xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
   
	k3=step*F;
	xnew=x+k3;

F=[ xnew(2) ;
    xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index) ;
    -((1/tau_1)+(1/tau_2))*(xnew(3)+ (K*tau_3/(tau_1*tau_2))*delta(index))-...
        (1/(tau_1*tau_2))*(abar*xnew(2)^3 + bbar*xnew(2)) + (K/(tau_1*tau_2))*delta(index) ];
   
	k4=step*F;
	x=x+(1/6)*(k1+2*k2+2*k3+k4); % Calculated next state


t=t+step;  			% Increments time
index=index+1;	 	% Increments the indexing term so that 
					% index=1 corresponds to time t=0.
counter=counter+1;	% Indicates that we computed one more integration step

end % This end statement goes with the first "while" statement 
    % in the program so when this is complete the simulation is done.

%
% Next, we provide plots of the input and output of the ship 
% along with the reference heading that we want to track.
%

% First, we convert from rad. to degrees
psi_r=psi_r*(180/pi);
psi=psi*(180/pi);
delta=delta*(180/pi);
e=e*(180/pi);
c=c*(180/pi);

% Next, we provide plots of data from the simulation

figure(5)
clf
subplot(211)
plot(time,psi,'k-',time,psi_r,'k--')
grid on
xlabel('Time (sec)')
title('Ship heading (solid) and desired ship heading (dashed), deg.')
subplot(212)
plot(time,delta,'k-')
grid on
xlabel('Time (sec)')
title('Rudder angle (\delta), deg.')
zoom

figure(6)
clf
subplot(211)
plot(time,e,'k-')
grid on
xlabel('Time (sec)')
title('Ship heading error between ship heading and desired heading, deg.')
subplot(212)
plot(time,c,'k-')
grid on
xlabel('Time (sec)')
title('Change in ship heading error, deg./sec')
zoom
	


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