npccon2.m

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

% ----------------------------------   NPCCON2.M    -------------------------------
%
%  This program simulates a predictive controller applied to a process
%  modelled by a nonlinear neural network model. 
%
%  Minimization of the predictive control criterion:
%
%                     N2                              Nu
%                    ---         ^        2           ---                    2
%       J(t,U)  =    >   (r(t+k)-y(t+k|t))   +  rho * >   (u(t+k-1)-u(t+k-2))
%                    ---                              --- 
%                    k=N1                             k=1
%
%  is done with a Newton based Levenberg-Marquardt algorithm similar to
%  the one described in R. Fletcher: "Practical Methods of Optimization",
%  Prentice-Hall, 1987
%
%  All design parameters must be defined in the file 'npcinit.m'
%
%  Programmed by Magnus Norgaard, IAU/EI/IMM, Technical Univ. of Denmark
%  LastEditDate: Oct. 28, 1997


%----------------------------------------------------------------------------------
%-------------------         >>>  INITIALIZATIONS  <<<        ---------------------
%----------------------------------------------------------------------------------

%>>>>>>>>>>>>>>>>>>>>>>      READ VARIABLES FROM FILE       <<<<<<<<<<<<<<<<<<<<<<<
clear plot_a plot_b
global ugl
npcinit
eval(['load ' nnfile]);                % Load neural network


% >>>>>>>>>>>>>>>>>>>>>>>>   DETERMINE REGRESSOR STRUCTURE   <<<<<<<<<<<<<<<<<<<<<<   
na      = NN(1);                       % # of past y's to be used in TDL
nb      = NN(2);                       % # of past u's to be used in TDL
nk      = NN(3);                       % Time delay in system
d       = NN(3);                       % Time delay in addition to the usual 1
N1      = d;                           % N1<>d not fully implemented yet. 
inputs  = na+sum(nb);                  % Total number of inputs to network
outputs = 1;                           % # of outputs is 1 (SISO system)
phi     = zeros(inputs,1);             % Initialize regressor vector


% >>>>>>>>>>>>>>>>>    DETERMINE STRUCTURE OF NETWORK MODEL     <<<<<<<<<<<<<<<<<<<
hidden   = length(NetDef(1,:));        % Number of hidden neurons
L_hidden = find(NetDef(1,:)=='L')';    % Location of linear hidden neurons
H_hidden = find(NetDef(1,:)=='H')';    % Location of tanh hidden neurons
y1       = zeros(hidden,N2-N1+1);      % Hidden layer outputs
yhat     = zeros(outputs,1);           % Network output


%>>>>>>>>>>>>>>>>>>>>>>>        INITIALIZE VARIABLES        <<<<<<<<<<<<<<<<<<<<<<
% Determine length of reference filter polynomials
nam = length(Am);
nbm = length(Bm);

% Initialization of past signals
maxlen = 5;
ref_old    = zeros(maxlen,1);
y_old      = zeros(N2,1);
yhat_old   = zeros(N2,1);
u_old      = zeros(maxlen,1);

% Initialization of constant gain PID parameters
if strcmp(regty,'pid'),
  B1 = K*(1+Ts*Wi/2);
  A1 = Ts*Wi;
  B2 = (2*Td+Ts)/(2*alf*Td+Ts);
  A2 = 2*Ts/(2*alf*Td+Ts);
  I1 = 0;
  I2 = 0;
  uimin = -10; uimax = 10;
end

% Miscellaneous initializations
df  = ones(hidden,1);
d2f = ones(hidden,1);
dUtilde_dU = eye(Nu);
dUtilde_dU(1:Nu-1,2:Nu)=dUtilde_dU(1:Nu-1,2:Nu)-eye(Nu-1);
d2U        = rho'*dUtilde_dU*dUtilde_dU';
dY_dU      = zeros(Nu,N2-N1+1);  % Initialize matrix of partial derivatives
hj_mat     = zeros(hidden*Nu,N2-d+1);
d2Y_dU2    = zeros(Nu*Nu,N2-d+1);

y0         = 0;                  % The output up to time t<=0
u          = 0;                  % The controls up to time t<=0
up         = u(ones(Nu,1));      % Initial future controls
upmin      = up;
index1     = 1:Nu:Nu*(Nu-1)+1;   % A useful vector
index2     = 1:hidden:hidden*(hidden-1)+1; %Another useful vector
index3     = 1:(Nu+1):Nu^2;      % A third useful vector

t = -Ts;
delta2     = delta*delta;
fighandle=progress;

% Initialization of Simulink system
if strcmp(simul,'simulink')
  simoptions = simset('Solver',integrator,'MaxRows',0); % Set integrator opt.
  eval(['[sizes,x0] = ' sim_model '([],[],[],0);']);
end


%>>>>>>>>>>>>>>>>    CALCULATE REFERENCE SIGNAL & FILTER IT     <<<<<<<<<<<<<<<<<<
if strcmp(refty,'siggener'),
  ref = zeros(samples+N2,1);
  for i = 1:samples+N2,
    ref(i) = siggener(Ts*(i-1),sq_amp,sq_freq,sin_amp,sin_freq,dc,sqrt(Nvar));
  end
elseif strcmp(refty,'none'),
  ref = zeros(samples+N2,1);
else
  eval(['ref = ' refty ';']);
  ref=ref(:);
  i=length(ref);
  if i>samples+N2,
    ref=ref(1:samples+N2);
  else
    ref=[ref;ref(i)*ones(samples+N2-i,1)];
  end
end
ref=filter(Bm,Am,ref);


% Initializations of vectors used for storing old data
ref_data    = ref(1:samples);
u_data      = zeros(samples,1);
y_data      = zeros(samples,1);
yhat_data   = zeros(samples,1);
t_data      = zeros(samples,1);
e_data      = zeros(samples,1);


% Data vectors used in control algorithm
% u_vec : First element is u(t-d+N1-nb+1), last element is u(t-d+N2)
% Index to time t is thus: tiu=d-N1+nb ("t index u_vec")
u_vec = u(ones(N2-N1+nb,1));
tiu   = d-N1+nb;
upi   = [1:Nu-1 Nu(ones(1,N2-d-Nu+2))];     % [1 2 ... Nu Nu ... Nu] 
uvi   = [tiu:N2-N1+nb];

% y_vec: First element is y(t-na), last element is y(t-1)
% Index to time t is: tiy=na+1
y_vec = y0(ones(na,1));
tiy   = na+1;

% yhat_vec: First element is yhat(t), last element is yhat(t+N2)
% Index to time t is: tiyh=1
yhat_vec = y0(ones(N2+1,1));
tiyh = 1;


%------------------------------------------------------------------------------
%-------------------         >>>   MAIN LOOP   <<<           ------------------
%------------------------------------------------------------------------------
for i=1:samples,
  t = t + Ts;
  
%>>>>>>>>>>>>>>>>>>>>>>>>    PREDICT OUTPUT FROM PLANT  <<<<<<<<<<<<<<<<<<<<<<<
  phi            = [y_vec(na:-1:1);u_old(d:d+nb-1)];
  h1             = W1(:,1:inputs)*phi + W1(:,inputs+1);  
  y1(H_hidden,1) = pmntanh(h1(H_hidden)); 
  y1(L_hidden,1) = h1(L_hidden);
  yhat           = W2(:,1:hidden)*y1(:,1) + W2(:,hidden+1);
  yhat_vec(tiyh) = yhat;


%>>>>>>>>>>>>>>>>>>>>>>>>    READ OUTPUT FROM PLANT     <<<<<<<<<<<<<<<<<<<<<<<
  if strcmp(simul,'simulink')
    utmp=[t-Ts,u_old(1);t,u_old(1)];
    simoptions.InitialState=x0;
    [time,x0,y] = sim(sim_model,[t-Ts t],simoptions,utmp);
    x0 = x0(size(x0,1),:)';
    y  = y(size(y,1),:)';
  elseif strcmp(simul,'matlab')
    ugl = u_old(1);
    [time,x] = ode45(mat_model,[t-Ts t],x0);
    x0 = x(length(time),:)';
    eval(['y = ' model_out '(x0);']);
  elseif strcmp(simul,'nnet')
    y = yhat;
  end

 
  %>>>>>>>>>>>>>>>>>>  FIND NEW CONTROL SIGNAL BY OPTIMIZATION  <<<<<<<<<<<<<<<<<
  upmin0   = upmin([2:Nu Nu]);
  einitval = eval(initval);     % Evaulate inival string
  for tr=1:length(einitval),
    up=upmin0;                  % Initial value for numerical search for a new u  
    up(Nu)=einitval(tr);
    u_vec(uvi) = up(upi);  
    dw = 1;                     % Flag specifying that up is new
    lambda = 0.1;               % Initialize Levenberg-Marquardt parameter
    m = 1; 
  
  
  %>>>>>>>>>>>>>>> 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  = ref(i+N1:i+N2) - yhat_vec(tiyh+N1:tiyh+N2);
    J = evec'*evec + rho*(duvec'*duvec);
  
   while m<=maxiter,
 
   if dw == 1,
 
    %>>>>>>>>>>>>>>>>>>>    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));
      d2f(H_hidden) = -2*y1(H_hidden,k-N1+1).*df(H_hidden);
      for l=0:min(k-d,Nu-2)
         %                                 min(k-d-l,na)
         %                                     ---        dy(t+k-1)
         % h(k,l,j) =    w              +      >   w      --------
         %                j,na+k-d-l+1         ---   j,i   du(t+l)
         %                                     i=1
         % 
         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
         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