npccon1.m

基于神经网络的控制工具箱

         y1(H_hidden,k-N1+1) = pmntanh(h1(H_hidden)); 
         y1(L_hidden,k-N1+1) = h1(L_hidden);
         yhat_vec(tiyh+k) = W2(:,1:hidden)*y1(:,k-N1+1) + W2(:,hidden+1);
       end
       
       %========================     EVALUATE CRITERION     =======================
       duvec = u_vec(tiu:tiu+Nu-1)-u_vec(tiu-1:tiu+Nu-2);
       evec  = ref(i+N1:i+N2) - yhat_vec(tiyh+N1:tiyh+N2);
       J_mu = evec'*evec + rho*(duvec'*duvec);


       %====================    DETERMINE dyhat(t+k,up_mu)/du   ===================
       for k=N1:N2
       % tanh'(x)
       df(H_hidden)  = (1-y1(H_hidden,k-N1+1).*y1(H_hidden,k-N1+1));

      for l=0:min(k-d,Nu-2)
         imax1 = min(k-d-l,na);
         if l>=k-d-nb+1,
          if imax1>=1, 
           hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                          + W1(:,na+k-d-l+1);;
          else
              hj_vec=W1(:,na+k-d-l+1);
          end
         else
           hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)';
         end
       dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
      end
      
      if k>=Nu
         l=Nu-1;
         imax1 = min(k-d-l,na);
         imax2 = min(k-d-Nu+2,nb);
         if imax2>1,
           if k==Nu,
             hj_vec = sum(W1(:,na+1:na+imax2)')';
           else
             hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                                                    + sum(W1(:,na+1:na+imax2)')';
           end
         else
           if k==Nu,
             hj_vec = W1(:,na+1:na+imax2);
           else
             hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                                                    + W1(:,na+1:na+imax2);
           end
         end                                    
 
         dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
       end 
     end
       
     
      %=========================     DETERMINE dJ/du     =========================
       dJdu_mu   = 2*(-dY_dU*evec + rho*(dUtilde_dU*duvec));
       Jb2 = J_mu; Gb2 = dJdu_mu;
     
     
       %========================  CHECK THE TWO CONDITIONS   ======================
       flag1 = 0;
       if (J_mu <= J + delta*mu*f'*dJdu),
         % step 2
         if (f'*dJdu_mu >= beta*f'*dJdu),
           break
         end
       else
         % Step 3
         flag1 = 1;
         break
       end
     
       % Step 4
       b1 = mu; b2 = 2*mu; mu=2*mu; Jb1 = Jb2; Gb1 = Gb2;
     end
   
     while flag1==1,
       % Step 5
       bb = b1-b2;
       p2 = (Jb2 - Jb1 + f'*Gb1*bb) / (bb*bb);  
       p1 = f'*Gb1 -2*p2*b1;
       beta = -0.5*p1/p2;

       % Step 6
       if ( min(beta-b1, b2-beta) >= 0.1*(b2-b1) )
         mu = beta;
       else
         mu = (b1+b2)/2;
       end
   
       % Step 7
       %==============  COMPUTE PREDICTIONS FROM TIME t+N1 TO t+N2  ===============
       up_mu = up + mu*f;
       u_vec(uvi) = up_mu(upi);          % Insert updated controls
       for k=N1:N2,
         phi              = [yhat_vec(tiyh+k-1:-1:tiyh+k-min(k,na)) ; ...
                  y_vec(tiy-1:-1:tiy-max(na-k,0)) ; u_vec(tiu-d+k:-1:tiu-d+1-nb+k)];
         h1               = W1(:,1:inputs)*phi + W1(:,inputs+1);  
         y1(H_hidden,k-N1+1) = pmntanh(h1(H_hidden)); 
         y1(L_hidden,k-N1+1) = h1(L_hidden);
         yhat_vec(tiyh+k) = W2(:,1:hidden)*y1(:,k-N1+1) + W2(:,hidden+1);
       end
       
       %========================     EVALUATE CRITERION     =======================
       duvec = u_vec(tiu:tiu+Nu-1)-u_vec(tiu-1:tiu+Nu-2);
       evec  = ref(i+N1:i+N2) - yhat_vec(tiyh+N1:tiyh+N2);
       J_mu = evec'*evec + rho*(duvec'*duvec);
       
       %===========================    DETERMINE dy/du   ==========================
       for k=N1:N2,
         % tanh'(x)
         df(H_hidden)  = (1-y1(H_hidden,k-N1+1).*y1(H_hidden,k-N1+1));

         for l=0:min(k-d,Nu-2)
           imax1 = min(k-d-l,na);
           if l>=k-d-nb+1,
          if imax1>=1, 
           hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                          + W1(:,na+k-d-l+1);;
          else
              hj_vec=W1(:,na+k-d-l+1);
          end
         else
           hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)';
         end
         dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
        end
      
      if k>=Nu
         l=Nu-1;
         imax1 = min(k-d-l,na);
         imax2 = min(k-d-Nu+2,nb);
         if imax2>1,
           if k==Nu,
             hj_vec = sum(W1(:,na+1:na+imax2)')';
           else
             hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                                                    + sum(W1(:,na+1:na+imax2)')';
           end
         else
           if k==Nu,
             hj_vec = W1(:,na+1:na+imax2);
           else
             hj_vec = W1(:,1:imax1)*dY_dU(l+1,k-N1:-1:k-imax1-N1+1)'...
                                                    + W1(:,na+1:na+imax2);
           end
         end                                    
 
         dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
       end 
     end
       
       
       %=========================    DETERMINE dJ/du     ==========================
       dJdu_mu   = 2*(-dY_dU*evec + rho*(dUtilde_dU*duvec));
     
     
       % Step 8
       %=======================   CHECK THE TWO CONDITIONS   ======================
       if (J_mu <= J + delta*mu*f'*dJdu),
         if (f'*dJdu_mu >= beta*f'*dJdu),
           break
         end
       
         % Step 9a
         b1 = mu;
         Jb1 = J_mu;
         Gb1 = dJdu_mu;
       else
         % Step 9b
         b2 = mu;
         Jb2 = J_mu;
         Gb2 = dJdu_mu;
       end
     end  
       
     % Step 10, Accept the iteration
     J = J_mu;
     dJdu_old = dJdu;
     dJdu = dJdu_mu;
   
  
     %>>>>>>>>>>>>>>>>>>>>>>>>   UPDATE FUTURE CONTROLS   <<<<<<<<<<<<<<<<<<<<<<<<<
     up_old = up;
     up = up_mu; 
     
     
     %>>>>>>>>>>>>>>>>>>>>>>>>     CHECK STOP CONDITION     <<<<<<<<<<<<<<<<<<<<<<<
     dup = up-up_old;
     if dup'*dup < delta2,
       break;
     end 
       
       
     %>>>>>>>>>>>>>>>>>>>     BFGS UPDATE OF INVERSE HESSIAN    <<<<<<<<<<<<<<<<<<
     dG  = dJdu - dJdu_old;
     BdG = B*dG;
     dupdG = dup'*dG;
     fac = 1/dupdG;
     diff = dup - BdG;
     dupfac=dup*fac;
     diffdup = diff*(dupfac'); 
     B = B + diffdup + diffdup' - (diff'*dG)*(dupfac*dupfac');
  end


    %>>>>>>>>>>>>>>>>>>>>>>>     SELECT BEST MINIMUM     <<<<<<<<<<<<<<<<<<<<<<<<<
    if tr==1,
      Jmin_old = J;
      upmin = up;
    else
      if J<Jmin_old,
        upmin = up;
      end
    end
  end


%>>>>>>>>>>>>>>>>>>>>>>     CALCULATE CONTROL SIGANL     <<<<<<<<<<<<<<<<<<<<<<
  e = ref(i) - y;
 
  % Predictive Controller
  if strcmp(regty,'npc'),
    u=upmin(1);

  % PID controller
  elseif strcmp(regty,'pid'),
    ui = B1*e + I1;
    um = ui;
    if ui<uimin, um=uimin; end
    if ui>uimax, um=uimax; end
    u = (um-I2)*B2 + I2;
    I1 = I1 + (K*e - (ui - um))*A1;
    I2 = I2 + (um - I2)*A2;
  
  % No control
  else
     u = ref(i);
  end
  
  
 %>>>>>>>>>>>>>>>>>>>       STORE DATA IN DATA VECTORS      <<<<<<<<<<<<<<<<<<<
  u_data(i)       = u;
  y_data(i)       = y;
  yhat_data(i)    = yhat_vec(tiyh);
  t_data(i)       = t;
  e_data(i)       = J;


%>>>>>>>>>>>>>>>>>>>>>>>>>>       TIME UPDATES        <<<<<<<<<<<<<<<<<<<<<<<<<
  y_old    = shift(y_old,y);
  u_old    = shift(u_old,u);
  u_vec(uvi) = upmin(upi);
  u_vec    = [u_vec(2:length(u_vec)) ; upmin(length(up))];
  y_vec    = [y_vec(2:length(y_vec)) ; y];
  yhat_vec(1:length(yhat_vec)-1) = yhat_vec(2:length(yhat_vec));


%>>>>>>>>>>>>>>>>>>      WRITE % OF SIMULATION COMPLETED      <<<<<<<<<<<<<<<<<
  progress(fighandle,floor(100*i/samples));
end
%------------------------------------------------------------------------------
%------------------        >>>   END OF MAIN LOOP   <<<       -----------------
%------------------------------------------------------------------------------

%>>>>>>>>>>>>>>>>>>>>>>            DRAW PLOTS           <<<<<<<<<<<<<<<<<<<<<<<
figure(gcf);clf
set(gcf,'DefaultTextInterpreter','none');

% Plot A
  if(exist('plot_a')==1),
   [a_plots,dummy]=size(plot_a);        % Number of plots in plot A
   plmat = zeros(samples,a_plots);      % Collect vectors in plmat
   for nn = 1:a_plots, 
     plmat(:,nn) = eval(plot_a(nn,:));   
   end
   subplot(2,1,1);
   plot([0:samples-1],plmat);           % Plot plmat
   xlabel('Samples');
   set(gca,'Xlim',[0 samples-1]);       % Set x-axis
   if regty(1)=='n',
     title('Nonlinear Predictive Control');
   elseif regty(1)=='p',
     title('Constant gain PID controller');
   else
     title('Open-loop simulation');
   end
   grid on
   legend(plot_a);
  end
  
 % Plot B
  if(exist('plot_b')==1),
   [b_plots,dummy]=size(plot_b);        % Number of plots in plot B
   plmat = zeros(samples,b_plots);      % Collect vectors in plmat
   for nn = 1:b_plots, 
     plmat(:,nn) = eval(plot_b(nn,:));   
   end
   subplot(2,1,2);
   plot([0:samples-1],plmat);           % Plot plmat
   xlabel('Samples'); 
   set(gca,'Xlim',[0 samples-1]);       % Set x-axis
   grid on
   legend(plot_b);
  end
set(gcf,'DefaultTextInterpreter','tex');
subplot(111)