vehiclegrad.m

用于汽车巡航控制系统的模糊控制算法

function VehicleGrad(NormIn, NormOut, ConstantDeriv, ConstraintFlag, NegateFlag, ...
   ParamFileIn, ParamFileOut, TrainFile)

% function VehicleGrad
% Optimize a fuzzy cruise control system using gradient descent.

% INPUTS
%   NormIn = 2-element array containing the range of each input variable.
%            If NormIn is passed to this routine, that means that this program is
%            run automatically for several values of NormIn, and the results
%            are automatically saved.  
%            Each element of NormIn should be a 2-element row vector.
%            If NormIn is not passed to this routine, then this program is
%            run for a single default input variable range.
%   NormOut = number containing the range of each output variable.
%             If NormOut is passed to this routine, that means that this program is
%             run automatically for several values of NormOut, and the results
%             are automatically saved.  
%             If NormOutis not passed to this routine, then this program is
%             run for a single default output variable range.
%   ConstantDeriv = flag (0 or 1) indicating whether the derivatives should be calculated
%                   just once at the first iteration.
%   ConstraintFlag = flag (0 or 1) indicating whether the fuzzy membership functions
%                    should be constrained to be sum normal.
%   NegateFlag = flag (0 or 1) indicating if gradients should be negated before use.
%   ParamFileIn = input parameter file
%   ParamFileOut = output parameter file
%   TrainFile = output training history file

DebugFlag = 0;

nclassin = [5 5];
nclassout = [5];
NormInDefault = [4 4];
NormOutDefault = 0.3;

% Define the gradient descent stopping critera.
epsilon = eps;
IterMax = 50;

if ~exist('ConstantDeriv','var')
   ConstantDeriv = 1;
end
if ~exist('ConstraintFlag','var')
   ConstraintFlag = 1;
end
if ~exist('NegateFlag','var')
   NegateFlag = 0;
end

EmptyParamFileIn = 0;
if exist('ParamFileIn','var')
   if isempty(ParamFileIn)
      EmptyParamFileIn = 1;
   end
else
   ParamFileIn = '';
end
EmptyParamFileOut = 0;
if exist('ParamFileOut','var')
   if isempty(ParamFileOut)
      EmptyParamFileOut = 1;
   end
else
   ParamFileOut = '';
end
if ~exist('TrainFile','var')
   TrainFile = '';
end

% Set the initial gradient descent step size.
if (ConstraintFlag == 1)
   etainit = 30;
else
   etainit = 10;
end

if exist('NormIn','var')
   if iscell(NormIn)
      NumTests = size(NormIn, 2);
   else
      clear NormIn;
      NumTests = 1;
   end
else
   NumTests = 1;
end
if exist('NormOut','var')
   if (NormOut == 0)
      clear NormOut;
   end
end

% Create the D matrix and d vector for the sum normal constraint.
[D, d] = DMatrix([nclassin nclassout]);
DPseudoInv = inv(D * D');

for iTest = 1 : NumTests
   
   % Set the initial gradient descent step sizes.
   eta = etainit;
   
   % Initialize the fuzzy membership functions.
   if exist('NormIn','var')
      [nin, nout, b, c, bout, cout, rule] = ...
         FuzzInit(NormIn{iTest}, NormOut(iTest), nclassin, nclassout);
   else
      [nin, nout, b, c, bout, cout, rule] = ...
         FuzzInit(NormInDefault, NormOutDefault, nclassin, nclassout);
   end
   
   % Check if the user wants to read initial fuzzy parameters from an input file.
   if exist('NormIn','var')
      filename = '';
   else
      sprompt = ['Enter the file name from which to read the fuzzy parameters.\n',...
            '(Enter a blank to use the default fuzzy parameters.)\n? '];
      
      if DebugFlag | EmptyParamFileIn
         filename = '';
      elseif ~isempty(ParamFileIn)
         filename = ParamFileIn;
      else
         filename = input(sprompt, 's');
      end
      
   end
   
   if ~isempty(filename)
      fid = fopen(filename, 'r');
      c = fscanf(fid, '%f', [nin, max(nclassin)]);
      b = fscanf(fid, '%f', [2 * nin, max(nclassin)]);
      b = reshape(b, [nin, max(nclassin), 2]);
      cout = fscanf(fid, '%f', [nout, max(nclassout)]);
      bout = fscanf(fid, '%f', [2 * nout, max(nclassout)]);
      bout = reshape(bout, [nout, max(nclassout), 2]);
      fclose(fid);
   end
   
   dedc = 0;
   dedb = 0;
   dedgamma = 0;
   dedbeta = 0;
   
   cinit = c;
   binit = b;
   coutinit = cout;
   boutinit = bout;
   
   Earray = [];
   Eold = inf;
   pause(1);
   disp(['Iteration # 1 / ', num2str(IterMax), ' ...']);
   
   t0 = clock;
   
   for iter = 1 : IterMax
      DerivFlag = ~ConstantDeriv | (iter == 1);
      dedcOld = dedc;
      dedbOld = dedb;
      dedgammaOld = dedgamma;
      dedbetaOld = dedbeta;
      
      if iter == 1
         t1 = clock;
      end
      
      [velocity, E, dedc, dedb, dedgamma, dedbeta] = ...
         VehicleControl(nin, nclassin, nout, nclassout, b, c, bout, cout, ...
         rule, DerivFlag);
      
      if iter == 1
         etime(clock,t1);
      end
      
      if NegateFlag
         dedc = -dedc;
         dedb = -dedb;
         dedgamma = -dedgamma;
         dedbeta = -dedbeta;
      end
      
      if (~DerivFlag)
         dedc = dedcOld;
         dedb = dedbOld;
         dedgamma = dedgammaOld;
         dedbeta = dedbetaOld;
      end
      
      Earray = [Earray E];
      if iter == 1
         Einit = E;
      end
      if iter == IterMax
         disp(['Error = ', num2str(E)]);
         break;
      else
         disp(['Error = ', num2str(E), ' - Iteration # ', num2str(iter+1), ...
               ' / ', num2str(IterMax), ' ...']);
      end
      % Check for convergence.
      if (Eold > E) & ((Eold - E) / Eold <= epsilon)
         %break;      
      end
      % If the error is increasing, then cut the gradient descent step size in half
      % and revert to the previous fuzzy parameters.
      % If the error is decreasing, then update the fuzzy parameters on the 
      % basis of the partial derivatives.
      if E > Eold
         c = cold;
         b = bold;
         cout = coutold;
         bout = boutold;
         eta = eta / 2;
         % If we've been though here too many times, the step size is so small
         % that we will not make any more progress in the optimization.
         if eta < etainit / 15 
            %break;
         end
      else
         if E < Eold
            eta = etainit;
         end
         Eold = E;
         cold = c;
         bold = b;
         coutold = cout;
         boutold = bout;
         if ConstraintFlag
            % Put the fuzzy membership parameters into a single xhat vector.
            % Then use the derivatives to compute a new xhat.
            xhat = [];
            dedx = [];
            for i = 1 : nin
               for j = 1 : max(nclassin)
                  xhat = [xhat b(i, j, 1) b(i, j, 2) c(i, j)]; 
                  dedx = [dedx dedb(i, j, 1) dedb(i, j, 2) dedc(i, j)]; 
               end
            end
            for i = 1 : nout
               for j = 1 : max(nclassout)
                  xhat = [xhat bout(i, j, 1) bout(i, j, 2) cout(i, j)]; 
                  dedx = [dedx dedbeta(i, j, 1) dedbeta(i, j, 2) dedgamma(i, j)]; 
               end
            end
            xhat = xhat - eta * dedx;
            xhat = xhat';
            % Constrain xhat so that the fuzzy membership functions are sum normal.
            xhat = xhat - D' * DPseudoInv * (D * xhat - d);
            % Extract the membership function parameters from xhat.
            k = 1;
            for i = 1 : nin
               for j = 1 : max(nclassin)
                  b(i, j, 1) = xhat(k);
                  k = k + 1;
                  b(i, j, 2) = xhat(k);
                  k = k + 1;
                  c(i, j) = xhat(k);
                  k = k + 1;
               end
            end
            for i = 1 : nout
               for j = 1 : max(nclassout)
                  bout(i, j, 1) = xhat(k);
                  k = k + 1;
                  bout(i, j, 2) = xhat(k);
                  k = k + 1;
                  cout(i, j) = xhat(k);
                  k = k + 1;
               end
            end
         else
            
            
            % Do not constrain the fuzzy membership functions to be sum normal.
            c = c - eta * dedc; 
            b = b - eta * dedb;
            cout = cout - eta * dedgamma;
            bout = bout - eta * dedbeta;         
            % Update the half-widths so that the membership functions remain sum normal.
            % The following code results in poor optimization performance, showing
            % the need for constrained optimization if we want the membership functions
            % to be sum normal.
            if (1 == 0)
               for i = 1 : nin
                  j = nclassin(i);
                  b(i, j, 1) = c(i, j) - c(i, j - 1);
                  b(i, 1, 2) = c(i, 2) - c(i, 1);
                  for j = 2 : nclassin(i) - 1
                     b(i, j, 1) = c(i, j) - c(i, j - 1);
                     b(i, j, 2) = c(i, j + 1) - c(i, j);
                  end
               end
               for i = 1 : nout
                  j = nclassout(i);
                  bout(i, j, 1) = cout(i, j) - cout(i, j - 1);
                  bout(i, 1, 2) = cout(i, 2) - cout(i, 1);
                  for j = 2 : nclassout(i) - 1
                     bout(i, j, 1) = cout(i, j) - cout(i, j - 1);
                     bout(i, j, 2) = cout(i, j + 1) - cout(i, j);
                  end
               end
            end
         end
      end
   end
   
   etime(clock,t0);
      
   plot(Earray);
   
   % Check if the user wants to write the fuzzy parameters to an output file.
   sprompt = ['Enter the file name to which to save the fuzzy parameters.\n',...
         '(Enter a blank if you do not want to save the fuzzy parameters.)\n? '];
   fid = -1;
   while fid < 0
      if exist('NormIn','var')
         filename = ['param', num2str(iTest, '%02d'), '.txt'];
      else
         
         if DebugFlag | EmptyParamFileOut
            if ConstraintFlag
               filename = 'paramgctemp.txt';
            else
               filename = 'paramgutemp.txt';
            end
         elseif ~isempty(ParamFileOut)
            filename = ParamFileOut;
         else
            filename = input(sprompt, 's');
         end
  
      end
      if isempty(filename)
         break;
      end
      fid = fopen(filename, 'w');
      sprompt = ['The file open attempt was unsuccessful.\n',...
            'Enter the file name to which to save the fuzzy parameters.\n',...
            '(Enter a blank if you do not want to save the fuzzy parameters.)\n? '];
   end
   if fid >= 0
      fprintf(fid, '%f\n', c);
      fprintf(fid, '%f\n', b);
      fprintf(fid, '%f\n', cout);
      fprintf(fid, '%f\n', bout);
      fclose(fid);
   end
   
   % Check if the user wants to write the errors to an output file.
   sprompt = ['Enter the file name to which to save the training errors.\n',...
         '(Enter a blank if you do not want to save the errors.)\n? '];
   fid = -1;
   while fid < 0
      if exist('NormIn','var')
         filename = ['err', num2str(iTest, '%02d'), '.txt'];
      else
         
         if DebugFlag
            if ConstraintFlag
               filename = 'traingctemp.txt';
            else
               filename = 'traingutemp.txt';
            end
         elseif ~isempty(TrainFile)
            filename = TrainFile;
         else
            filename = input(sprompt, 's');
         end
         
      end
      if isempty(filename)
         break;
      end
      fid = fopen(filename, 'w');
      sprompt = ['The file open attempt was unsuccessful.\n',...
            'Enter the file name to which to save the training errors.\n',...
            '(Enter a blank if you do not want to save the errors.)\n? '];
   end
   if fid >= 0
      fprintf(fid, '%f\n', Earray);
      fclose(fid);
   end
   
   c = abs(c - cinit);
   sizec = size(c);
   deltac = sum(sum(sum(c.*c))) / sizec(1) / sizec(2);
   b = abs(b - binit);
   sizeb = size(b);
   deltab = sum(sum(sum(b.*b))) / sizeb(1) / sizeb(2) / sizeb(3);
   cout = abs(cout - coutinit);
   sizecout = size(cout);
   deltacout = sum(sum(sum(cout.*cout))) / sizecout(1) / sizecout(2);
   bout = abs(bout - boutinit);
   sizebout = size(bout);
   deltabout = sum(sum(sum(bout.*bout))) / sizebout(1) / sizebout(2) / sizebout(3);
   disp(['Error change = ', num2str(100*(Einit-E)/Einit), '%, ' ...
         'Ave c change = ', num2str(deltac), ', Ave b change = ', ...
         num2str(deltab)]);
   disp(['Ave cout change = ', num2str(deltacout), ...
         ', Ave bout change = ', num2str(deltabout)]);
   disp(['The fuzzy cruise control optimization has completed.']);
   
end