%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Multilayer Perceptron for Tanker Ship Heading Regulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% By: Kevin Passino
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear		% Clear all variables in memory

vary = [1,2,3,4,5,6,7,8,9,10];
for fun = 1:length(vary)

Kp = vary(fun);
Kd = 20;

% 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 multilayer perceptron

% The first hidden layer is trivial, with unity weights and a zero bias

% Weights/biases in the second hidden layer:

w112=10;
w122=10;
b12=-200*pi/180;
b22=+200*pi/180;

% Weights/biases in the third hidden layer:

w113=-80*pi/180;
w223=-80*pi/180;
b13=0;
b23=80*pi/180;

% The bias in the output layer is zero and the two
% output layer weights are unity.


% Now, you can proceed to do the simulation or simply view the nonlinear
% surface generated by the neural controller.

%flag1=input('\n Do you want to simulate the \n neural control system \n for the tanker?  \n (type 1 for yes and 0 for no) ');

%if flag1==1,

% 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

        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 conventinal proportional-derivative controller:
        delta(index)= Kp * -e(index) + Kd * -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 a new control output 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);
        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);

% Next, we provide plots of data from the simulation

figure(1)
clf
subplot(311)
plot(time,psi,'k-',time,psi_r,'k--')
grid on
title('Ship heading (solid) and desired ship heading (dashed), deg.')
subplot(312)
plot(time,e,'k-')
grid on
title('Ship heading error between ship heading and desired heading, deg.')
subplot(313)
plot(time,delta,'k-')
grid on
xlabel('Time (sec)')
title('Rudder angle (\delta), deg.')
zoom


%end % This ends the if statement (on flag1) on whether you want to do a simulation
% or just see the control surface

%end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of program %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (fun ==1)
    save('ship_data(1)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==2)
    save('ship_data(2)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==3)
    save('ship_data(3)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==4)
    save('ship_data(4)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==5)
    save('ship_data(5)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==6)
    save('ship_data(6)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==7)
    save('ship_data(7)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==8)
    save('ship_data(8)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==9)
    save('ship_data(9)','psi','psi_r','delta', 'Kp', 'Kd');
elseif (fun ==10)
    save('ship_data(10)','psi','psi_r','delta', 'Kp', 'Kd');
end
end

S=stepinfo(psi(1:2000));

Tr2=S.RiseTime;
Ts2=S.SettlingTime;
Po2=S.Overshoot;