contr.m

非线性控制 Matlab编译

function u=contr(y,goaly, goalu, mode,ampl,niter,m,mc,nd, neighbors)
% U=CONTR(Y, GOALY, GOALU, MODE, AMPL, NITER, N, NC, D, NEIGHBORS)
% Multiple-Input-Multiple-Output adaptive, nonlinear controller.
% returns U - controlling perturbations (column vector NUx1)
% upon receving system output Y (column vector NYx1).
% The controller objective is set in the vector GOALY (NYx1) of desired 
% system outputs.  Setting it to Inf is equivalent to setting (diff(Y)=0),
% convenient for steady state stabilization when the exact position of the
% steady state is unknown.
% GOALU sets the goal values of the controller outputs.  Setting it to Inf
% causes the controller to move the system to GOALY values, while diff(U)=0.
% NOTE: Setting both GOALY and GOALU to Inf doesn't make sense;
% set them both to real values if they are available. 
%
%   MODE:0-identification stage:  the controller just applies random
%      perturbations
%      and collects system responses. AMPL (NUx1) sets the amplitude of
%      identification perturbations vector. 
%      The controller will switch to mode 1 after collecting NITER readings,
%	 1-control of the objective state. 
%	 2-adaptive control:  controlling perturbations are superimposed
%           with random excitations (amlitude AMPL). 
%           Last NITER responses are stored and used.
%   AMPL - NUx1 vector, sets the amplitude of identification perturbation
%        or maximum allowed perturbations in MODE 1.
%   IMPORTANT! Perturbations have to be scaled such that the characteristic
%   amplitudes of perturbations and observations are the same. 
%   This is due to the simple Euclidian distance measure used for the nearest
%   neighbors selection. 
%
%   NITER - sets the length of the identification sequence; make it at least
% 20*DIM^2 (DIM - dimensionality of the system). It has to be considerably
% larger than NEIGHBORS.
% Controller parameters:
%   N - length of time-delayed/forwarded signals for system state
%       reconstruction, usually N =(>) DIM/NY. 
%   NC - length of the controlling sequence, usually NC =(<) DIM/NU
%   D - delay time before applied perturbations reach the system
%       output. D = 0 will always work but the controller will not be optimal.
%   NEIGHBORS - number of nearest neighbors in the phase space used
%   for the linear interpolation of the control surface. It has to be at
%   least 10*DIM
% N, NC, and D determine the structure of the controller and have to be set
% either on the basis of knowledge of the controlled system, or by trial
% and error. It is best to start with N ~ expected dimensionality/NY, NC=N, D=0
% Setting N to be more than 10 may require too many data points
% before the algorithm will be usable.

% Requires: idseq.m - forms vector of state transitions
%	    linint.m - a linear hypersurface interpolator (vector form)
%	    delaysig2.m - an extended version of delaysig.m
%	    ctrseq.m - forms vector of transition to the objective

% Written by V. Petrov, 1998
% e-mail: [e-mail]
%
% Copyright (c) 1998 The University of Texas at Austin

global XX UU P T K ua;

ccase=0;

if(sum(isinf(goaly)))
 ccase=1;
 goaly=0*ones(size(y,1),1);
end

if(sum(isinf(goalu)))
 ccase=2;
 goalu=0*ones(size(ampl,1),1);
end


li=mc+2*m+2*nd+1;  %Number of most recent records to keep

if (length(K)==0) 
 K=0;
 ua=0*ones(size(ampl));
 XX=0*ones(size(y))*ones(1,li);
 UU=0*ones(size(ampl))*ones(1,li);
end


K = K+1;

%%%%%%%%%%%%%%%%% Build the reference set %%%%%%%%%%%%%%%%%%% 
 XX(:,1:li-1)=XX(:,2:li);
 UU(:,1:li-1)=UU(:,2:li);
 XX(:,li)=y;
 UU(:,li) = ua;

% Rearrange the data to construct the hypersurface
if ( K > li )
 if (length(P) == 0)  %If first time, create P and T
  [P,T]=idseq(XX,UU,m,mc,nd,ccase);
  P=P(:,size(P,2));  
  T=T(:,size(T,2));
 else
  [P2,T2]=idseq(XX,UU,m,mc,nd,ccase);
  if (size(P,2) < niter )
   P=[P P2(:,size(P2,2))];  % Add the last measurement
   T=[T T2(:,size(T2,2))];
  else % If NITER recordings are made
   if (mode == 0) mode=1; end
   if (mode == 2) 
%% If in adaptive regime, remove from the begining, add to the end %%%
    P=[P(:,2:niter) P2(:,size(P2,2))];  
    T=[T(:,2:niter) T2(:,size(T2,2))];
   end
  end
 end

 if (mode > 0 )  %Create present-target state vector for control
  PC=ctrseq(XX,UU,goaly, goalu, m,mc,nd, ccase);
 end

end

% Identification
if (mode == 0 ) 
  u=ampl.*randn(size(ampl));
end


if (mode > 0 ) 
    u=linint(P,T,PC,neighbors);

end

if (mode == 2 ) 
  u=u+ampl.*randn(size(ampl));
end

if (mode == 1 ) 
  in=find((abs(u)-ampl)>0);  %Find if any of the perturbations exceed the limit
  if (length(in) > 0)
   u(in)=sign(u(in)).*ampl(in); %Restrict large perturbations
  end
end



ua = u;