% control design for MC 
% July 2, 1997

clear all

% parameters

my=3;                               % number of the states of y and u "my" 
rand('seed',39);
T=500;                              % similation length
T0=3;                               % memory length of tank system
randn('seed',13)
F=3;                                % filter number
m=10;                                % discretization interval number
Xbar=eye(1,2*F);                    % expectation of regressor
sigma=10;
P=sigma*eye(2*F,2*F);                      % P=(X-EX)*(X-EX)'
P0=P;
lambda=0.99;
nu=T0;                                  % forgetting time
H=10;                                   % control horizon

% initial state Xbar0    

for i=1:F
Xbar0(i)=1;
Xbar0(i+F)=0;
end
        
% initial states

for t=1:T0
Y(t)=4;                       % initial states of system output
U(t)=1;                       % initial states of system input
y(t)=1;
end
U(T0+1)=0.7;
%n=zeros(my*F*(m-2),my^2);                  % sufficient statistics
n0=0.005*ones(my*F*(m-2),my^2);
sita=(1/(0.005*my))*n0;
n=n0;

% set point
    
    IY=1;
    
% reference value of input
  
    IU=0;

% boundary of signals

    Yupper=10;
    Ylower=0;
    Uupper=5;
    Ulower=-5;
    IY=(IY-(Yupper+Ylower)/2)/((Yupper-Ylower)/2);   
    IU=(IU-(Uupper+Ulower)/2)/((Uupper-Ulower)/2);
     
% optimal controller
  
   for t=T0+1:T-1
     v0=(1/(m-2)*F)*ones(1,(m-2)*F);                % pooling weight
     v=v0;
     pred=zeros(1,(m-2)*F);

     Y(t)=system(Y,U,t,Yupper,Ylower);                   % simulated system
     Y1(t)=(Y(t)-(Yupper+Ylower)/2)/((Yupper-Ylower)/2);  % rescaling
     U1(t)=(U(t)-(Uupper+Ulower)/2)/((Uupper-Ulower)/2);
     for i=1:F
     X(i)=Y1(t-i+1);
     IX(i)=IY;
     X(i+F)=U1(t-i+1);
     IX(i+F)=IU;
     end
     nu=lambda*(nu+1);
     Xbar=lambda*Xbar+(X-Xbar)/(nu)+(1-lambda)*Xbar0;     
     P=lambda*P+((X-Xbar)'*(X-Xbar)-P)/(nu)+(1-lambda)*P0;
     R=chol(P);
   for fil=1:F                         % filter loop
     fy(t)=filt1(X,fil,R,F);
     Ify(t)=filt1(IX,fil,R,F);
     FY(fil,t)=fy(t);
     fu(t)=U1(t);
   for dis=1:m-2                       % discretization loop
     [y(t),IS]=discry1(y,fy,my,t,dis,m,Ify);
     [u(t),IC]=discru1(u,fu,my,t,dis,m,IU);
     [n,sita,pred]=identi(n0,y,u,t,my,n,m,F,fil,dis,lambda,sita,pred);
     c=design(sita,IS,IC,my,fil,dis,m,H);
     contr(:,dis+(m-2)*(fil-1))=finer(c(:,y(t)),dis,m);
   end
   end
     v=weig(pred,m,F,lambda,v0,v);
     mp=pool(contr,m,F,v);
     U(t+1)=contin1(mp,m,Ulower,Uupper);
   end
   
hold off

plot(Y);