% control design for MC - mk 
% Jan 30, 1998

clear all

ico=1;                             % switch between controlled (1) 
                                   %         and uncontrolled (-1) case

% parameters

my=3;                               % the number of the states of the output y  
mu=3;                               % the number of the states of the  input u
if ico>0
  m=3;                             % the number of discretization intervals
  F=3;                              % the number of filters ("memory")
else
  m=3;
  F=3;
end  
lambda=0.9;                         % forgetting factor

%simulation

T=500;                              % simulation length
U=zeros(1,T);                       % continuous inputs
Y=zeros(1,T);                       % scaled continuous outputs
U1=zeros(1,T);                      % scaled continuous inputs
Y1=zeros(1,T);                      % continuous outputs
fy=Y;                               %filtered continuous outputs
Ify=Y;                              %ideal filtered continuous outputs
fu=U;                               %filtered continuous inputs

T0=3;                               % memory length of the system
Y(1:T0)=-1*ones(1,T0);             % initial outputs
U(1:T0)=-5*ones(1,T0);               % initial inputs
U(T0+1)=-7;                     
randn('seed',13)
%filtering

Xbar0=[ones(1,F),zeros(1,F)];       %initial estimate of continuous state X
Xbar=Xbar0;                         %sample mean of continuous state X
X=Xbar;                             %continuous state
sigma=10;                           %guess of the sample dispersions of X
P0=sigma*eye(2*F,2*F);              %guess of the sample covariance  of X
P=P0;                               %sample value of P=E(X-EX)*(X-EX)'
nu0=T0;                             %the initial number of degrees of freedom
nu=nu0;                             %the number of degrees of freedom

%estimation

y=ones(1,T);                        %discrete outputs
u=ones(1,T);                        %discrete inputs

n0=0.005*ones(my*F*(m-2),my*mu);    %initial value of sufficient statistics
n=n0;                               %sufficient statistics
sita=n0/sum(n0(1:my,1));            %parameter estimate

%control 

H=10;                               % control horizon
rand('seed',39);
IY=3;                               % set point of continuous output
IU=0;                               % reference value of  continuous input
c=zeros(mu,my);                     % a local controller
contr=zeros(m,(m-2)*F);             % all local controllers 
%scaling
Yupper=5;                          % boundaries of continuous output
Ylower=-2;
Uupper=1;                           % boundaries of continuous output
Ulower=-10;
                                    % set point of scaled continuous output
IY1=(IY-(Yupper+Ylower)/2)/((Yupper-Ylower)/2); 
                                    % set point of scaled continuous input
IU1=(IU-(Uupper+Ulower)/2)/((Uupper-Ulower)/2);  
Y1(1:T0)=(Y(1:T0)-(Yupper+Ylower)/2)/((Yupper-Ylower)/2); % scaling initail state
U1(1:T0)=(U(1:T0)-(Uupper+Ulower)/2)/((Uupper-Ulower)/2);
fy=Y1;                               %filtered continuous outputs
Ify(1:T0)=IY1*ones(1,T0);                              %ideal filtered continuous outputs
fu=U1;                               %filtered continuous inputs

IX=X;                               % discrete ideal state

%pooling
v0=(1/(m-2)/F)*ones(1,(m-2)*F);     % initial pooling weights
v=v0;                               % pooling weights
pred=zeros(1,(m-2)*F);              % predictors

%simulation loop

for t=T0+1:T-1

     Y(t)=msystem(Y,U,t,Yupper,Ylower);                    % simulated system
%     Y
     Y1(t)=(Y(t)-(Yupper+Ylower)/2)/((Yupper-Ylower)/2);  % scaling
%     Y1
     U1(t)=(U(t)-(Uupper+Ulower)/2)/((Uupper-Ulower)/2);  % scaling
%     U1
    if ico>0 
     for i=1:F
       X(i)=Y1(t-i);              % normalized state 
       X(i+F)=U1(t-i+1);
       IX(i)=IY1;                    % ideal discrete state
       IX(i+F)=IU1;
     end 
%     X
%     IX
%sample statistics
     nup=nu;
     nu=lambda*(nu+1)+(1-lambda)*nu0; 
     Xbar=lambda*(nup*Xbar+X)/nu+(1-lambda)*Xbar0;     
     P=lambda*(nup*P+(X-Xbar)'*(X-Xbar))/nu+(1-lambda)*P0;
     R=inv(chol(P)');
   end  
%filtering     
     for fil=1:F                           %filter loop
      if ico>0
       R(fil,:)=R(fil,:)/norm(R(fil,:),1); %normalization
       fy(t)=R(fil,:)*X';                  %output of fil-th filter
       Ify(t)=R(fil,:)*IX';                %ideal output of fil-th filter
 %      fy
%       Ify
       
%discretization, estimation, control design
       for dis=1:m-2                       % discretization loop
         [y(t),IS]=mdiscry(fy(t),dis,m,my,mu,Ify(t));
         [u(t),IC]=mdiscru(U1(t),dis,m,my,mu);
         [n,sita,pred]=midenti(n0,y,u,t,my,n,m,fil,dis,lambda,sita,pred);
         c=mdesign(sita,IS,IC,my,mu,fil,dis,m,H);
         contr(:,dis+(m-2)*(fil-1))=mfiner(c(:,y(t)),dis,m);
       end
      end
     end 
     if ico>0
      v=mweig(pred,m,lambda,F,v0,v);
      mp=mpool(contr,m,F,v);
      U(t+1)=mcontin2(mp,m,Ulower,Uupper);
     else
      U(t+1)=0;                            %uncontrolled case for comparison
     end
end

%drawing
hold off
plot(Y(1:T-1));
hold on
plot(U(1:T-1),'r');

crity=norm((Y-IY)/T,1)
critu=norm(U/T,1)