final.m

Neural network based ship heading control

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Radial Basis Function Neural Controller for Tanker Ship Heading Regulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% By: Kevin Passino 
% Version: 1/21/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)
abar=1;             % Parameters for nonlinearity
bbar=1;

% Define the reference model (we use a first order transfer function 
% k_r/(s+a_r)):

a_r=1/150;
k_r=1/150;

% Adaptation gain:

eta=1;

% Parameters for reinforcement function:

eta_e=1;
eta_c=20;

% 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

% Define spreads of Gaussian functions

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

% Next, pick the *initial* strengths for the receptive field units (these are what will
% later be adjusted by the reinforcement learning method): 

% First, you could use the approach from the neural networks chapter:

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

% Another choice is just to use all zero strengths - to test how good it is at synthesizing the
% initial controller.

thetaold=0*theta;

% phi for the RBF NN is initialized below

% Compute vectors with points over the whole range of 
% the neural controller inputs - for use below

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

% Convert from radians to degrees:

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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=20000;	% Stopping time for the simulation (in seconds) - normally 20000
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.  
cold=0;     % Need this to initialize phiold below

psi_r_old=0; % Initialize the reference trajectory
yeold=0; 	 % Intial condition used to calculate yc
ymold=0; 	 % Initial condition for the first order reference model

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.

% Need to initialize phi

for i=1:nR
	phiold(i,1)=exp(-(((eold-center(1,i))^2)/sigmae^2)-(((cold-center(2,i))^2)/sigmac^2));
end


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

psi_r=0*ones(1,tstop+1);
psi=0*ones(1,tstop+1);
e=0*ones(1,tstop+1);
c=0*ones(1,tstop+1);
s=0*ones(1,tstop+1);
w=0*ones(1,tstop+1);
delta=0*ones(1,tstop+1);
ym=0*ones(1,tstop+1);
J_R=0*ones(1,tstop+1);
ye=0*ones(1,tstop+1);
yc=0*ones(1,tstop+1);

while t <= tstop

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

if t>=0, 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>=1500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=3000, psi_r(index)=45*(pi/180); end    % Request heading of -45 deg
if t>=4500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=6000, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>=7500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=9000, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>=10500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=12000, psi_r(index)=45*(pi/180); end    % Request heading of -45 deg
if t>=13500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=15000, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>=16500, psi_r(index)=0; end    			% Request heading of 0 deg
if t>=18000, psi_r(index)=45*(pi/180); end     % Request heading of 45 deg
if t>=19500, psi_r(index)=0; end    			% Request heading of 0 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

% Reference model calculations:
% The reference model is part of the controller and to simulate it
% we take the discrete equivalent of the
% reference model to compute psi_m from psi_r (if you use
% a continuous-time reference model you will have to augment 
% the state of the closed-loop system with the state(s) of the 
% reference model and hence update the state in the Runge-Kutta 
% equations).
%
% For the reference model we use a first order transfer function 
% k_r/(s+a_r) but we use the bilinear transformation where we 
% replace s by (2/step)(z-1)/(z+1), then find the z-domain 
% representation of the reference model, then convert this 
% to a difference equation:

ym(index)=(1/(2+a_r*T))*((2-a_r*T)*ymold+...
                                    k_r*T*(psi_r(index)+psi_r_old));

ymold=ym(index);  
psi_r_old=psi_r(index);

	% This saves the past value of the ym and psi_r so that we can use it
	% the next time around the loop
	

% 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, perform calculations for reinforcement signal

ye(index)=ym(index)-psi(index);		    % Calculates ye
yc(index)=(ye(index)-yeold)/T;			% Calculates yc
yeold=ye(index);					    % Saves the value of ye for use the 
							            % next time

% Compute the reinforcement signal:

J_R(index)=eta*(-eta_e*ye(index)-eta_c*yc(index));

% When reinforcement signal is very small, simply make it zero (in
% this way it will not over-react to small deviations in adjusting
% the controller - it will only make adjustments when they are really needed)

if abs(J_R(index))<0.005
	J_R(index)=0;
end

% Compute the adjustments to the strengths

for i=1:nR
	theta(i,1)=thetaold(i,1)+J_R(index)*phiold(i,1);
end

% Next, compute the phi vector for the next time around the loop

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

thetaold=theta(:,1); % Save this for next time around the loop
phiold=phi(:,1); % Save this for next time so that in the above formula the indices
                 % for thetaold and phiold are the same

% Compute the RBF output

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


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);
ye(index)=ye(index-1);