%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Response Surface Methodology for Design of a
% Proportional-Derivative Controller for 
% Tanker Ship Heading Regulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% By: Kevin Passino 
% Version: 3/20/01
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear		% Clear all variables in memory

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

% Proportional and derivative gains (the input space for the design)

%Kp=-1.5; Some reasonable size gains - found manually
%Kd=-250;

Kpmin=-5;
Kpmax=-0.5;  % Program below assumes this

Kdmin=-500;
Kdmax=-100; % Program below assumes this


% Define grid of design points (over (Kp,Kd) space)

nGKp=20;   % The number of partitions on Kp dimension
nGKd=20;   % The number of partitions on Kd dimension
Npoints=nGKp*nGKd % The total number of design points

n=2; % The number of inputs 

Kp=Kpmin:(Kpmax-Kpmin)/(nGKp-1):Kpmax;  % Define a uniformly spaced vector roughly on the input domain
			             				% that is used to form the grid on the (Kp,Kd) space
Kd=Kdmin:(Kdmax-Kdmin)/(nGKd-1):Kdmax;  

k=0; % Counter initialization

% Place the centers on a grid

KpKd=0*ones(nGKp,nGKd); % Initialize

for i=1:length(Kp)
	for j=1:length(Kd)
	  k=k+1;
	  KpKd(1,k)=Kp(i);
	  KpKd(2,k)=Kd(j);
	end
end

% Plot the design points of the grid

figure(1)
clf
plot(KpKd(1,:),KpKd(2,:),'ko')
grid on
xlabel('K_p gain')
ylabel('K_d gain')
title('Grid of design points')
axis([Kpmin Kpmax Kdmin Kdmax])


% Store performance measure for evaluating closed-loop performance (this 
% will define the response surface)

Jcl=0*ones(nGKd,nGKp); % Allocate memory

% Define scale parameters for performance measure
w1=1;
w2=0.01;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start loop for generating data for response surface

for ii=1:length(Kp)
	for jj=1:length(Kd)

	  flag=1; % Indicates nominal conditions
%	  flag=0; % Indicates off-nominal conditions
	  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the controller 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=1200;	% 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
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.


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

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.
%if flag==0, s(index)=0.01*(pi/180)*(2*rand-1); else
s(index)=0;					  % This allows us to remove the noise.
%end

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
	

% 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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A proportional-derivative controller:

delta(index)=Kp(ii)*e(index)+Kd(jj)*c(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 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);

ym(index)=ym(index-1);

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

% 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

%if flag==0, 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);
%end

% 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

% The next line is used in place of the line following it to
% change the speed of the ship
%if flag==0,
%%if t>=1000000,
%%if t>=9000,      % This switches the ship speed (unrealistically fast)
%u=3; % A lower speed

%else

u=5;

%end

% Next, we change the parameters of the ship to tanker to reflect
% changing loading conditions (note that we simulate as if
% the ship is loaded while moving, but we only change the parameters
% while the heading is zero so that it is then similar to re-running
% the simulation, i.e., starting the tanker operation at different 
% times after loading/unloading has occurred).

% The next line is used in place of the line following it to keep
% "ballast" conditions throughout the simulation
if flag==0, 
%t>=1000000,
%if t>=0,      % This switches the parameters, possibly in the middle of the simulation
K_0=0.83;  		% These are the parameters under "full" conditions
tau_10=-2.88;
tau_20=0.38;
tau_30=1.07;

else

K_0=5.88;		% These are the parameters under "ballast" conditions
tau_10=-16.91;
tau_20=0.45;
tau_30=1.43;

end

% The following parameters are used in the definition of the tanker model:

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


% 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, 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
% Compute how well the controller did in terms of deviation
% from the reference model and the control energy

Jcl(jj,ii)=w1*(psi-ym)*(psi-ym)'+w2*delta*delta';
	
end 
	
end % End response surface data generation loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the response surface
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(2)
clf
surf(Kp,Kd,Jcl);
%view(145,30);
zoom
colormap(white);
xlabel('K_p gain');
ylabel('K_d gain');
zlabel('J_c_l performance measure');
title('Response surface');
rotate3d



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Next, compute the best point on the surface:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[val,rowindex]=min(min(Jcl)); % Gives the min value (val) and its row index
[val,colindex]=min(min(Jcl')); % Gives the min value and its column index

% Save for later use:

Jclnominal=Jcl;

% Best gain

KpKdbest=[Kp(rowindex); Kd(colindex)]

figure(3)
clf
surf(Kp,Kd,Jcl);
hold on
plot3(KpKdbest(1,1),KpKdbest(2,1),Jcl(colindex,rowindex),'bo');
%view(145,30);
hold off
zoom
colormap(white);
xlabel('K_p gain');
ylabel('K_d gain');
zlabel('J_c_l performance measure');
title('Response surface');
rotate3d



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

flag=1;
%flag=0; % Test under off-nominal conditions

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the controller 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=1200;	% 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
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.


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

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.
%if flag==0, s(index)=0.01*(pi/180)*(2*rand-1); else
s(index)=0;					  % This allows us to remove the noise.
%end

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
	

% 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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A proportional-derivative controller:

delta(index)=KpKdbest(1,1)*e(index)+KpKdbest(2,1)*c(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 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);

ym(index)=ym(index-1);

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

% 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

%if flag==0, 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);
%end

% 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

% The next line is used in place of the line following it to
% change the speed of the ship
%if t>=1000000,
%if t>=9000,      % This switches the ship speed (unrealistically fast)
%if flag==0,
%u=3; % A lower speed

%else

u=5;

%end

% Next, we change the parameters of the ship to tanker to reflect
% changing loading conditions (note that we simulate as if
% the ship is loaded while moving, but we only change the parameters
% while the heading is zero so that it is then similar to re-running
% the simulation, i.e., starting the tanker operation at different 
% times after loading/unloading has occurred).

% The next line is used in place of the line following it to keep
% "ballast" conditions throughout the simulation
%if t>=1000000,
%if t>=9000,      % This switches the parameters in the middle of the simulation
if flag==0,
K_0=0.83;  		% These are the parameters under "full" conditions
tau_10=-2.88;
tau_20=0.38;
tau_30=1.07;

else

K_0=5.88;		% These are the parameters under "ballast" conditions
tau_10=-16.91;
tau_20=0.45;
tau_30=1.43;

end

% The following parameters are used in the definition of the tanker model:

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


% 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, 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 data from the simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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);
ym=ym*(180/pi);


% Next, we provide plots showing the performance of the best design

figure(4)
clf
subplot(211)
plot(time,psi,'k-',time,ym,'k--',time,psi_r,'k-.')
zoom
grid on
title('Ship heading (solid) and desired ship heading (dashed), deg.')
subplot(212)
plot(time,delta,'k-')
zoom
grid on
title('Rudder angle, output of controller (input to the ship), deg.')
xlabel('Time, sec')

figure(5)
clf
plot(time,psi-ym,'k-')
zoom
grid on
title('Ship heading error between ship heading and reference model heading, deg.')
xlabel('Time, sec')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Just repeat above code, but for off-nominal case
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start loop for generating data for response surface

for ii=1:length(Kp)
	for jj=1:length(Kd)

	  flag=1; % Indicates nominal conditions
	  flag=0; % Indicates off-nominal conditions
	  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the controller 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=1200;	% 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
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.


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

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.
%if flag==0, s(index)=0.01*(pi/180)*(2*rand-1); else
s(index)=0;					  % This allows us to remove the noise.
%end

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
	

% 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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A proportional-derivative controller:

delta(index)=Kp(ii)*e(index)+Kd(jj)*c(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 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);

ym(index)=ym(index-1);

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

% 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

%if flag==0, 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);
%end

% 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

% The next line is used in place of the line following it to
% change the speed of the ship
%if flag==0,
%%if t>=1000000,
%%if t>=9000,      % This switches the ship speed (unrealistically fast)
%u=3; % A lower speed

%else

u=5;

%end

% Next, we change the parameters of the ship to tanker to reflect
% changing loading conditions (note that we simulate as if
% the ship is loaded while moving, but we only change the parameters
% while the heading is zero so that it is then similar to re-running
% the simulation, i.e., starting the tanker operation at different 
% times after loading/unloading has occurred).

% The next line is used in place of the line following it to keep
% "ballast" conditions throughout the simulation
if flag==0, 
%t>=1000000,
%if t>=0,      % This switches the parameters, possibly in the middle of the simulation
K_0=0.83;  		% These are the parameters under "full" conditions
tau_10=-2.88;
tau_20=0.38;
tau_30=1.07;

else

K_0=5.88;		% These are the parameters under "ballast" conditions
tau_10=-16.91;
tau_20=0.45;
tau_30=1.43;

end

% The following parameters are used in the definition of the tanker model:

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


% 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, 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
% Compute how well the controller did in terms of deviation
% from the reference model and the control energy

Jcl(jj,ii)=w1*(psi-ym)*(psi-ym)'+w2*delta*delta';
	
end 
	
end % End response surface data generation loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the response surface
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(6)
clf
surf(Kp,Kd,Jcl);
%view(145,30);
zoom
colormap(white);
xlabel('K_p gain');
ylabel('K_d gain');
zlabel('J_c_l performance measure');
title('Response surface');
rotate3d



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Next, compute the best point on the surface:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[val,rowindex]=min(min(Jcl)); % Gives the min value (val) and its row index
[val,colindex]=min(min(Jcl')); % Gives the min value and its column index

% Same the surface:
Jcloffnominal=Jcl;

% Best gain

KpKdbest=[Kp(rowindex); Kd(colindex)]

figure(7)
clf
surf(Kp,Kd,Jcl);
hold on
plot3(KpKdbest(1,1),KpKdbest(2,1),Jcl(colindex,rowindex),'bo');
%view(145,30);
hold off
zoom
colormap(white);
xlabel('K_p gain');
ylabel('K_d gain');
zlabel('J_c_l performance measure');
title('Response surface');
rotate3d



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

flag=1;
flag=0; % Test under off-nominal conditions

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate the controller 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=1200;	% 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
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.


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

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.
%if flag==0, s(index)=0.01*(pi/180)*(2*rand-1); else
s(index)=0;					  % This allows us to remove the noise.
%end

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
	

% 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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A proportional-derivative controller:

delta(index)=KpKdbest(1,1)*e(index)+KpKdbest(2,1)*c(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 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);

ym(index)=ym(index-1);

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

% 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

%if flag==0, 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);
%end

% 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

% The next line is used in place of the line following it to
% change the speed of the ship
%if t>=1000000,
%if t>=9000,      % This switches the ship speed (unrealistically fast)
%if flag==0,
%u=3; % A lower speed

%else

u=5;

%end

% Next, we change the parameters of the ship to tanker to reflect
% changing loading conditions (note that we simulate as if
% the ship is loaded while moving, but we only change the parameters
% while the heading is zero so that it is then similar to re-running
% the simulation, i.e., starting the tanker operation at different 
% times after loading/unloading has occurred).

% The next line is used in place of the line following it to keep
% "ballast" conditions throughout the simulation
%if t>=1000000,
%if t>=9000,      % This switches the parameters in the middle of the simulation
if flag==0,
K_0=0.83;  		% These are the parameters under "full" conditions
tau_10=-2.88;
tau_20=0.38;
tau_30=1.07;

else

K_0=5.88;		% These are the parameters under "ballast" conditions
tau_10=-16.91;
tau_20=0.45;
tau_30=1.43;

end

% The following parameters are used in the definition of the tanker model:

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


% 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, 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 data from the simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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);
ym=ym*(180/pi);


% Next, we provide plots showing the performance of the best design

figure(8)
clf
subplot(211)
plot(time,psi,'k-',time,ym,'k--',time,psi_r,'k-.')
zoom
grid on
title('Ship heading (solid) and desired ship heading (dashed), deg.')
subplot(212)
plot(time,delta,'k-')
zoom
grid on
title('Rudder angle, output of controller (input to the ship), deg.')
xlabel('Time, sec')

figure(9)
clf
plot(time,psi-ym,'k-')
zoom
grid on
title('Ship heading error between ship heading and reference model heading, deg.')
xlabel('Time, sec')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Next, find the best PD gains for both cases
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Next, compute the best point on the surface:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Jcombined=Jclnominal+Jcloffnominal;

[val,rowindex]=min(min(Jcombined)); % Gives the min value (val) and its row index
[val,colindex]=min(min(Jcombined')); % Gives the min value and its column index

% Best gain (for both conditions)

KpKdbest=[Kp(rowindex); Kd(colindex)]

figure(10)
clf
surf(Kp,Kd,Jcombined);
hold on
plot3(KpKdbest(1,1),KpKdbest(2,1),Jcombined(colindex,rowindex),'bo');
%view(145,30);
hold off
zoom
colormap(white);
xlabel('K_p gain');
ylabel('K_d gain');
zlabel('J_c_l performance measure, two conditions');
title('Response surface');
rotate3d


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