vehiclekalman.m

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

function VehicleKalman(NormIn, NormOut, P0, Q0, R0, ConstantH, ConstraintFlag, ...
   NegateFlag, ParamFileIn, ParamFileOut, TrainFile)

% function VehicleKalman(NormIn, NormOut, P0, Q0, R0, ConstantH)
% Optimize a fuzzy cruise control system using a Kalman filter.

% 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 = range of the 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 NormOut is not passed to this routine, then this program is
%             run for a single default output variable range.
%   P0 = initial magnitude of each diagonal element of the estimation error covariance.
%   Q0 = process noise covariance.
%   R0 = measurement noise covariance.
%   ConstantH = flag (0 or 1) indicating whether a constant value of H should be used
%               (partial of fuzzy output with respect to membership function parameters).
%   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

close all;

DebugFlag = 0;

% Define the number of input and output classes for each fuzzy variable.
nclassin = [5 5];
nclassout = [5];

NormInDefault = [4 4];
NormOutDefault = 0.3;

% Define the stopping critera.
epsilon = 0.0001;
IterMax = 10;
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 covariance matrices.
if ~exist('P0','var') | (P0 == 0)
   if ConstraintFlag == 1
      P0 = 1e18;
   else
      P0 = 1e6;
   end
elseif P0 < 0
   PO = 0;
end
if ~exist('Q0','var') | (Q0 == 0)
   if (ConstraintFlag == 1)
      Q0 = 4000;
   else
      Q0 = 4000;
   end
elseif Q0 < 0
   Q0 = 0;
end
if ~exist('R0','var') | (R0 == 0)
   if (ConstraintFlag == 1)
      R0 = 1e-8;
   else
      R0 = 1;
   end
elseif R0 < 0
   R0 = 0;
end

NumTests = 1;
if exist('NormIn','var')
   if iscell(NormIn)
      NumTests = size(NormIn, 2);
   else
      clear NormIn;
   end
end
if exist('NormOut','var')
   if NormOut == 0
      NormOut = NormOutDefault;
   end
end
if ~exist('ConstantH','var')
   ConstantH = 1;
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

	% 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
   
   cinit = c;
   binit = b;
   coutinit = cout;
   boutinit = bout;
   
   % Compute the number of Kalman filter states.
   n = 3 * (nin * max(nclassin) + nout * max(nclassout));
   
   % Initialize the estimation and noise covariance matrices.
   P = P0 * eye(n);
   Q = Q0 * eye(n);
   R = R0 * eye(nout);
   NumPDoubles = 0;
   NumImprovements = 0;
   NumImprovementsOld = 0;
   
   % Initialize the state estimate.
   xhat = [];
   for i = 1 : nin
	   for j = 1 : max(nclassin)
         xhat = [xhat b(i, j, 1) b(i, j, 2) c(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)]; 
      end
   end
   xhat = xhat';
   xtilde = xhat;
   
   Earray = [];
   NormParray = [];
   Eold = inf;
   ExitFlag = 0;
   pause(1);
   disp(['Iteration # 1 / ', num2str(IterMax), ' ...']);
   
   t0 = clock;
   
   for iter = 1 : IterMax
      DerivFlag = (mod(iter-1,1000) == 0) | ~ConstantH;
%      if DerivFlag & ~ConstantH
%         P = P0 * eye(n);
%		end
		if ConstraintFlag & ExitFlag
   		% Constrain the membership functions to be sum normal.
   		xhat = xhat - D' * DPseudoInv * (D * xhat - d);
		end
		% Extract the membership functions from the state estimate.
      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
      [velocity, E, dedc, dedb, dedgamma, dedbeta] = ...
         VehicleControl(nin, nclassin, nout, nclassout, b, c, bout, cout, ...
         rule, DerivFlag);
      
      if NegateFlag
         dedc = -dedc;
         dedb = -dedb;
         dedgamma = -dedgamma;
         dedbeta = -dedbeta;
      end
      
      Earray = [Earray E];
      NormParray = [NormParray norm(P)];
      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
      if ExitFlag
         break;
      end
      % Check for convergence.
      if ((Eold > E) & ((Eold - E) / Eold <= epsilon)) | (E >= Eold)
%         break;      
%	   elseif E >= Eold
         % The error is increasing, so increase the covariance
         % and revert to the previous fuzzy parameters.
         if (E >= Eold)
            xhat = xhatold;
         end
         P = P0 * eye(n);
         if ConstantH == 0
            R = 2 * R;
         end
         if (NumImprovements <= (NumImprovementsOld + 1))
            NumPDoubles = NumPDoubles + 1;
         else
            NumPDoubles = 1;
         end
         NumImprovementsOld = NumImprovements;
         % If we've been though here too many times, give up.
         if NumPDoubles > 3 
            %ExitFlag = 1;
%            E = Eold;
         else
            Eold = E + eps;
         end
      else
	      % The error is decreasing, so update the fuzzy parameters on the 
         % basis of the partial derivatives (using a Kalman filter).
         NumImprovements = NumImprovements + 1;
        	Eold = E;
         xhatold = xhat;
         if DerivFlag
	         H = [];
	         for i = 1 : nin
	            for j = 1 : max(nclassin)
	               H = [H dedb(i, j, 1) dedb(i, j, 2) dedc(i, j)];
	            end
            end
            for i = 1 : nout
               for j = 1 : max(nclassout)
                  H = [H dedbeta(i, j, 1) dedbeta(i, j, 2) dedgamma(i, j)];
               end
            end
            H = -H';
         end
         K = P * H * inv(R + H' * P * H);
         P = P - K * H' * P + Q;
         xhat = xhat + K * E;
         if ConstraintFlag
            xhat = xhat - D' * DPseudoInv * (D * xhat - d);
			end
         % Update the half-widths so that the membership functions remain sum normal.
         % The following code results in poor optimization performance.
         %      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
      end
   end
   
   etime(clock,t0);
      
   figure;
   plot(sqrt(2*Earray));
%   figure;
%   plot(NormParray);
   
   Directory = cd;
   
   % 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 = [Directory, '\param', num2str(iTest, '%02d'), '.txt'];
      else
         
         if DebugFlag | EmptyParamFileOut
            if ConstraintFlag
               filename = 'paramkctemp.txt';
            else
               filename = 'paramkutemp.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 = [Directory, '\err', num2str(iTest, '%02d'), '.txt'];
      else
         
         if DebugFlag
            if ConstraintFlag
               filename = 'trainkctemp.txt';
            else
               filename = 'trainkutemp.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(['Norm(P) = ', num2str(norm(P))]);
   disp(['The fuzzy cruise control optimization has completed.']);
   
end