nc_tanker.m

一个用MATLAB编写的优化控制工具箱

c(index)=c(index-1); 
delta(index)=delta(index-1);
ye(index)=ye(index-1);
yc(index)=yc(index-1);
J_R(index)=J_R(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

%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

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

% Plot the mapping in the middle of the simulation:

if t==8999
	
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_outputd1=delta_output*(180/pi);

figure(1)
clf
surf(e_inputd,c_inputd,delta_outputd1);
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

end


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

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

% Next, we provide plots 

figure(2)
clf
subplot(311)
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(312)
plot(time,delta,'k-')
zoom
grid on
title('Rudder angle, output of neural controller (input to the ship), deg.')
subplot(313)
plot(time,J_R,'k-')
zoom
grid on
xlabel('Time (sec)')
title('Reinforcement signal (nonzero values indicate adaptation)')

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

figure(4)
clf
subplot(211)
plot(time,ye,'k-')
zoom
grid on
title('Ship heading error between ship heading and reference model heading, deg.')
subplot(212)
plot(time,yc,'k-')
zoom
grid on
xlabel('Time (sec)')
title('Change in heading error between output and reference model, deg./sec')



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

%flag1=1;
%if flag1==0
	

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 final controller map

delta_outputd=delta_output*(180/pi);

figure(5)
clf
surf(e_inputd,c_inputd,delta_outputd);
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

%end

% Plot the difference between the two plots at the middle and end of the simulation


figure(6)
clf
surf(e_inputd,c_inputd,delta_outputd-delta_outputd1);
view(145,30);
colormap(white);
xlabel('Heading error (e), deg.');
ylabel('Change in heading error (c), deg.');
zlabel('Controller output (\delta), deg.');
title('Difference between mapping shapes');
rotate3d

% To view it better, create a contour plot

figure(7) 
% Neg indicates decreased size of map by end, pos indicates that increased
% Jet option below will give red for increases, blue for decreases
% Gray will give light for positive, dark for negative (use this to be consistent
% with GA plots)
clf
contour(e_inputd,c_inputd,delta_outputd-delta_outputd1,20)
%colormap(jet)
colormap(gray);
xlabel('Heading error (e), deg.');
ylabel('Change in heading error (c), deg.');
title('Difference between mapping shapes, contour map');



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