fmrlc_tanker.m

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

% of the fuzzy controller are on (this is the fuzzification part).

c_count=0;,e_count=0;   % These are used to count the number of 
						% non-zero mf certainities of e and c
e(index)=psi_r(index)-psi(index);	
			% Calculates the error input for the fuzzy controller
			
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
				
% The following if-then structure fills the vector mfe 
% with the certainty of each membership fucntion of e for the 
% current input e.  We use triangular membership functions.

	if e(index)<=ce(1)		% Takes care of saturation of the left-most 
					        % membership function
         mfe=[1 0 0 0 0 0 0 0 0 0 0]; % i.e., the only one on is the 
         							  % left-most one
	 e_count=e_count+1;,e_int=1; 	  %  One mf on, it is the 
	 								  % left-most one.
	elseif e(index)>=ce(nume)				  % Takes care of saturation 
									  % of the right-most mf
	 mfe=[0 0 0 0 0 0 0 0 0 0 1];
	 e_count=e_count+1;,e_int=nume; % One mf on, it is the 
	 								% right-most one
	else      % In this case the input is on the middle part of the 
			  % universe of discourse for e
			  % Next, we are going to cycle through the mfs to 
			  % find all that are on
	   for i=1:nume
		 if e(index)<=ce(i)
		  mfe(i)=max([0 1+(e(index)-ce(i))/we]);   
		  				% In this case the input is to the 
		  				% left of the center ce(i) and we compute
						% the value of the mf centered at ce(i)
						% for this input e
			if mfe(i)~=0	
				% If the certainty is not equal to zero then say 
				% that have one mf on by incrementing our count
			 e_count=e_count+1;
			 e_int=i;	% This term holds the index last entry 
			 			% with a non-zero term
			end
		 else 
		  mfe(i)=max([0,1+(ce(i)-e(index))/we]); 
		  						% In this case the input is to the
		  						% right of the center ce(i)
			if mfe(i)~=0
			 e_count=e_count+1;   
			 e_int=i;  % This term holds the index of the 
			 		   % last entry with a non-zero term
			end
		 end
		end
	end


% The following if-then structure fills the vector mfc with the 
% certainty of each membership fucntion of the c
% for its current value (to understand this part of the code see the above 
% similar code for computing mfe).  Clearly, it could be more efficient to
% make a subroutine that performs these computations for each of 
% the fuzzy system inputs.

	if c(index)<=cc(1)	% Takes care of saturation of left-most mf
         mfc=[1 0 0 0 0 0 0 0 0 0 0];
	 c_count=c_count+1;
	 c_int=1;   
	elseif c(index)>=cc(numc)	
				        % Takes care of saturation of the right-most mf
	 mfc=[0 0 0 0 0 0 0 0 0 0 1];
	 c_count=c_count+1; 
	 c_int=numc;
	else
		for i=1:numc
		 if c(index)<=cc(i)  
		  mfc(i)=max([0,1+(c(index)-cc(i))/wc]);
			if mfc(i)~=0
			 c_count=c_count+1;
			 c_int=i;  	    % This term holds last entry 
			 		     	% with a non-zero term
			end
		 else 
		  mfc(i)=max([0,1+(cc(i)-c(index))/wc]);
			if mfc(i)~=0
			 c_count=c_count+1;
			 c_int=i;	% This term holds last entry 
			 			% with a non-zero term
			end
		 end
		end
	end


% The next two loops calculate the crisp output using only the non-
% zero premise of error,e, and c.  This cuts down computation time 
% since we will only compute the contribution from the rules that 
% are on (i.e., a maximum of four rules for our case).  The minimum
% is used for the premise (and implication for the center-of-gravity
% defuzzification case).

num=0;
den=0;     
	for k=(e_int-e_count+1):e_int  
						% Scan over e indices of mfs that are on
		for l=(c_int-c_count+1):c_int	
						% Scan over c indices of mfs that are on
		  prem=min([mfe(k) mfc(l)]); 
		  				% Value of premise membership function
% This next calculation of num adds up the numerator for the 
% center of gravity defuzzification formula. rules(k,l) is the output center 
% for the rule.  base*(prem-(prem)^2/2 is the area of a symmetric
% triangle with base width "base" and that is chopped off at 
% a height of prem (since we use minimum to represent the 
% implication). Computation of den is similar but without 
% rules(k,l).
		  num=num+rules(k,l)*base*(prem-(prem)^2/2);
		  den=den+base*(prem-(prem)^2/2);		
% To do the same computations, but for center-average defuzzification,
% use the following lines of code rather than the two above (notice
% that in this case we did not use any information about the output
% membership function shapes, just their centers; also, note that
% the computations are slightly simpler for the center-average defuzzificaton):
%		num=num+rules(k,l)*prem;
%		den=den+prem;
		end
	end

   delta(index)=num/den;   
   			% Crisp output of fuzzy controller that is the input
   			% to the plant.
			
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, we perform computations for the fuzzy inverse model.

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

ye_count=0;,yc_count=0;    	% Counts the non-zero certainties 
							% of ye and yc similar to how we did 
							% for the fuzzy controller

% The following if-then structure fills the vector mfye with the 
% certainty of each membership fucntion of ye (similar to the 
% fuzzy controller).  Notice that we use the same number of 
% input membership functions as in the fuzzy controller - 
% just for convenience - it does not have to be this way

	if ye(index)<=cye(1)			% Takes care of saturation
         mfye=[1 0 0 0 0 0 0 0 0 0 0];
	 ye_count=ye_count+1;,ye_int=1;
	elseif ye(index)>=cye(numye)	% Takes care of saturation
	 mfye=[0 0 0 0 0 0 0 0 0 0 1];
	 ye_count=ye_count+1;,ye_int=numye;
	else
		for i=1:numye
		 if ye(index)<=cye(i)
		  mfye(i)=max([0 1+(ye(index)-cye(i))/wye]);
			if mfye(i)~=0
			 ye_count=ye_count+1;
			 ye_int=i;	% This term holds last entry with 
			 			% a non-zero term
			end
		 else 
		  mfye(i)=max([0,1+(cye(i)-ye(index))/wye]);
			if mfye(i)~=0
			 ye_count=ye_count+1;
			 ye_int=i;	% This term holds last entry with 
			 			% a non-zero term
			end
		 end
		end
	end

% The following if-then structure fills the vector mfyc with the 
% certainty of each membership fucntion of yc

	if yc(index)<=cyc(1)
         mfyc=[1 0 0 0 0 0 0 0 0 0 0];
	 yc_count=yc_count+1;,yc_int=1;
	elseif yc(index)>=cyc(numyc)
	 mfyc=[0 0 0 0 0 0 0 0 0 0 1];
	 yc_count=yc_count+1;,yc_int=numyc;
	else
		for i=1:numyc
		 if yc(index)<=cyc(i)
		  mfyc(i)=max([0 1+(yc(index)-cyc(i))/wyc]);
			if mfyc(i)~=0
			 yc_count=yc_count+1;
			 yc_int=i;
			end
		 else 
		  mfyc(i)=max([0,1+(cyc(i)-yc(index))/wyc]);
			if mfyc(i)~=0
			 yc_count=yc_count+1;
			 yc_int=i;
			end
		 end
		end
	end

% These for loops calculate the crisp output of the inverse model 
% using only the non-zero premise of error,ye, and change in 
% error,yc.  This cuts down computation time (similar to the 
% fuzzy controller). Minimum is used for the premise and the 
% implication.  To understand this part of the code see the 
% similar code for the fuzzy controller (of course you could use
% center-average defuzzification).

invnum=0;
invden=0;     
	for k=(ye_int-ye_count+1):ye_int
		for l=(yc_int-yc_count+1):yc_int			
		  prem=min([mfye(k) mfyc(l)]);
		  invnum=invnum+inverrules(k,l)*invbase*(prem-(prem)^2/2);
		  invden=invden+invbase*(prem-(prem)^2/2);
		end
	end
	
% Next we compute the output of the fuzzy inverse model.

if gp==0, 
	p(index)=0;				% Allow us to turn off the learning
else
  p(index)=invnum/invden;   % Crisp output of inverse model
  if abs(p(index))<0.01*gp, % Robustification term which is used
      p(index)=0;			% to stop the adaptation when the fuzzy
  end						% controller is close to where it should
							% be.
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, we modify the centers of ouput membership functions 
% associated with the rules that were on d steps ago
% The indexing sheme is similar to the one used in the 
% computation of the outputs of the fuzzy controller and 
% fuzzy inverse model. Notice that in the early steps these
% two loops would not be executed since we initialized
% meme_int, meme_count, memc_int, and memc_count with all zeros.
% This implies that there are no updates until d executions of 
% the controller (e.g., for our case it executes the controller 
% at the first step and then on the next controller execution which
% occurs at 10 sec. the rules will be modified).

	for k=(meme_int(d)-meme_count(d)+1):meme_int(d)
		for l=(memc_int(d)-memc_count(d)+1):memc_int(d)			
		  rules(k,l)=rules(k,l)+p(index);
		end
	end

% Note: Here, do not save the entire rule base for the past d steps
% so this implies that, e.g., for a large d value, between d steps ago 
% and the present time the rules could be modified and so the way this 
% is coded it would modify the one that was modified in this case, less
% than d steps ago.  To avoid this you would have to save all the rules 
% over the past d steps and when you modify you may be undoing what was
% done on a previous step - and it is not clear that would be good either.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Store values of pointers/counters for rules that were on at this step

% Next we will save the number of mfs that are on and the pointer 
% e_int as to which rules were on.  This vector of length 
% 10 saves the last 10 values of e_count and e_int as time 
% progresses (hence, it is a moving window).
% These will be used by the FMRLC knowledge-base modifier.

meme_count=[e_count meme_count(1:9)];
meme_int=[e_int meme_int(1:9)];

% Next we will save the number of mfs that are on and the pointer 
% c_int as to which rules were on.  This vector of length 10 
% saves the last 10 values of c_count and e_int as time progresses 
% (hence, it is a moving window). These will be used by the FMRLC 
% knowledge-base modifier.

memc_count=[c_count memc_count(1:9)];
memc_int=[c_int memc_int(1:9)];

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 FMRLC 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.  Hence, we need to 
% compute these here (note that we simply hold the values):

e(index)=e(index-1);	
c(index)=c(index-1); 
delta(index)=delta(index-1);
ye(index)=ye(index-1);
yc(index)=yc(index-1);
p(index)=p(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).
% (This does not implement the exact equations in the book but
% if you think about the problem a bit you will see that these
% really should be accurate enough. Here, we are not calculating
% intermediate values of the controller output, only
% one per integration step; we do this simply to save some 
% computations.  Similarly, for the reference input.)
  
	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