npccon2.m

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

         dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
      end
    
      if k>=Nu
         l=Nu-1;
         %       min(k-d-Nu+2,nb)          min(k-d-l,na)
         %             ---                    ---        dy(t+k-1)
         % h(k,l,j) =  >   w           +      >   w      --------
         %             ---  j,na+i            ---   j,i   du(t+l)
         %             i=1                    i=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
         hj_mat(index2(l+1):index2(l+1)+hidden-1,k-d+1) = hj_vec;
                                                
                                                    
         %          hidden     
         % dy(t+k)   ---
         % ------- =  >  W  * f'(y1(j)) * h(k,l,j)
         % du(t+l)   ---  j
         %           j=1     
         dY_dU(l+1,k-N1+1)  = W2(1:hidden)*(df.*hj_vec);
       end 
     end
   
   
     %>>>>>>>>>>>>>>>>>>>    DETERMINE d^2 y / du^2 <<<<<<<<<<<<<<<<<<<<<
     for k=d:N2,
       for l=0:Nu-1,
         imax1 = min(k-d-l,na);
         for p=0:l,
           if imax1>=1,
             tmpvec = W1(:,1:imax1)*d2Y_dU2(index1(l+1)+p,k-d:-1:k-imax1-d+1)';
           else
             tmpvec = zeros(hidden,1);
           end         
           dd = d2f.*hj_mat(index2(l+1):index2(l+1)+hidden-1,k-d+1)...
                                  .*hj_mat(index2(p+1):index2(p+1)+hidden-1,k-d+1);
           dd = dd + df.*tmpvec;
           d2Y_dU2(index1(l+1)+p,k-d+1) = W2(1:hidden)*dd;
         end
       end
     end
   
   
     %>>>>>>>>>>>>>>>>>>>    DETERMINE Hessian matrix <<<<<<<<<<<<<<<<<<<<<
     H = dY_dU*dY_dU' + d2U;
     for l=1:Nu,
       for p=1:l,
         H(l,p) = H(l,p) - d2Y_dU2(index1(l)+p-1,:)*evec;
         H(p,l) = H(l,p);              % This is due to the symmetry 
       end
     end
   
     % -- Gradient --
     dJdu   = -dY_dU*evec + rho*(dUtilde_dU*duvec);
     dw = 0;   
   end

%>>>>>>>>>>>>>>>>>>>>>   DETERMINE SEARCH DIRECTION   <<<<<<<<<<<<<<<<<<<<<
 
    % -- Add lambda to the diagonal --
    H(index3) = H(index3) + lambda;
    
    % -- Check if H is positive definite --
    p=1;
    while p~=0,
      [ch,p] = chol(H);
      if p~=0,         % Hessian + diagonal not positive definite
         lambda = lambda*4;
         H(index3) = H(index3) + lambda;
      end
    end
    
    % -- Determine search direction --
    f = -(ch\(ch'\dJdu));

    % -- Compute 'apriori' iterate --
    up_new = up + f;
    u_vec(uvi) = up_new(upi);          % Insert updated controls


     %>>>>>>>>>>>>> COMPUTE PREDICTIONS FROM TIME t+N1 TO t+N2  <<<<<<<<<<<<<<<< 
     for k=N1:N2,
       %----- Determine prediction yhat(t+k) -----
       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_new  = ref(i+N1:i+N2) - yhat_vec(tiyh+N1:tiyh+N2);
     J_new = evec_new'*evec_new + rho*(duvec'*duvec);


     %>>>>>>>>>>>>>>>>>>>>>>>       UPDATE lambda      <<<<<<<<<<<<<<<<<<<<<<<<<<
     L = -f'*dJdu +0.5*lambda*f'*f;
     
     % Reduce lambda if SSE has decreased 'sufficiently'
     if (J - J_new) > (0.75*L),
       lambda = lambda/2;
  
     % Increase lambda if SSE has increased 'sufficiently'
     elseif (J-J_new) <= (0.25*L),
       lambda = 2*lambda;
     end
     

     %>>>>>>>>>>>>>>>>>>>>>   UPDATES FOR NEXT ITERATION   <<<<<<<<<<<<<<<<<<<<<< 
     % Update only if criterion has decreased
     if J_new < J,
       evec = evec_new;
       du = up_new-up;                
       up = up_new;
       J = J_new;
       dw = 1;
       m = m + 1;
       % Check if stop condition is satisfied 
       if sqrt(du'*du)<delta2, break, end 
     end

     if lambda>1e3, break, end       
    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)