%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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 nonlinearitybbar=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 thisKdmin=-500;Kdmax=-100; % Program below assumes this% Define grid of design points (over (Kp,Kd) space)nGKp=20;   % The number of partitions on Kp dimensionnGKd=20;   % The number of partitions on Kd dimensionNpoints=nGKp*nGKd % The total number of design pointsn=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) spaceKd=Kdmin:(Kdmax-Kdmin)/(nGKd-1):Kdmax;  k=0; % Counter initialization% Place the centers on a gridKpKd=0*ones(nGKp,nGKd); % Initializefor i=1:length(Kp)	for j=1:length(Kd)	  k=k+1;	  KpKd(1,k)=Kp(i);	  KpKd(2,k)=Kd(j);	endend% Plot the design points of the gridfigure(1)clfplot(KpKd(1,:),KpKd(2,:),'ko')grid onxlabel('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 measurew1=1;w2=0.01;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Start loop for generating data for response surfacefor 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 zeroindex=1;	% This is time's index (not time, its index).  tstop=1200;	% Stopping time for the simulation (in seconds) - normally 20000step=1;     % Integration step sizeT=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 belowpsi_r_old=0; % Initialize the reference trajectoryymold=0; 	 % Initial condition for the first order reference modelx=[0;0;0];	% First, set the state to be a vector            x(1)=0;		% Set the initial heading to be zerox(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 degif t>=100, psi_r(index)=45*(pi/180); end     % Request heading of 45 degif t>=1500, psi_r(index)=0; end    			% Request heading of 0 degif t>=3000, psi_r(index)=45*(pi/180); end    % Request heading of -45 degif t>=4500, psi_r(index)=0; end    			% Request heading of 0 degif t>=6000, psi_r(index)=45*(pi/180); end     % Request heading of 45 degif t>=7500, psi_r(index)=0; end    			% Request heading of 0 degif t>=9000, psi_r(index)=45*(pi/180); end     % Request heading of 45 degif t>=10500, psi_r(index)=0; end    			% Request heading of 0 degif t>=12000, psi_r(index)=45*(pi/180); end    % Request heading of -45 degif t>=13500, psi_r(index)=0; end    			% Request heading of 0 degif t>=15000, psi_r(index)=45*(pi/180); end     % Request heading of 45 degif t>=16500, psi_r(index)=0; end    			% Request heading of 0 degif t>=18000, psi_r(index)=45*(pi/180); end     % Request heading of 45 degif 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); elses(index)=0;					  % This allows us to remove the noise.%endpsi(index)=x(1)+s(index);     % Heading of the ship (possibly with sensor noise).if counter == 10,  % When the counter reaches 10 then execute the 				   % controllercounter=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 ceold=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 degreesif delta(index) >= 80*(pi/180), delta(index)=80*(pi/180); endif 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%elseu=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 simulationif flag==0, %t>=1000000,%if t>=0,      % This switches the parameters, possibly in the middle of the simulationK_0=0.83;  		% These are the parameters under "full" conditionstau_10=-2.88;tau_20=0.38;tau_30=1.07;elseK_0=5.88;		% These are the parameters under "ballast" conditionstau_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 statet=t+step;  			% Increments timeindex=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 stepend % This end statement goes with the first "while" statement     % in the program so when this is complete the simulation is done.%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%