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: vpetrov@cybernet.mit.edu%% Copyright (c) 1998 The University of Texas at Austinglobal XX UU P T K ua;ccase=0;if(sum(isinf(goaly))) ccase=1; goaly=0*ones(size(y,1),1);endif(sum(isinf(goalu))) ccase=2; goalu=0*ones(size(ampl,1),1);endli=mc+2*m+2*nd+1;  %Number of most recent records to keepif (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);endK = 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 hypersurfaceif ( 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); endend% Identificationif (mode == 0 )   u=ampl.*randn(size(ampl));endif (mode > 0 )     u=linint(P,T,PC,neighbors);endif (mode == 2 )   u=u+ampl.*randn(size(ampl));endif (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  endendua = u;