u=contr(y,goaly, goalu, mode,ampl,niter,m,mc,nd, neighbors) 
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)
% MultipleInputMultipleOutput 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:0identification 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,
% 1control of the objective state.
% 2adaptive 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 timedelayed/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
% email: vpetrov@cybernet.mit.edu
%
% 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:li1)=XX(:,2:li);
UU(:,1:li1)=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 presenttarget 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;

