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

clear all

diary marout

% parameters

my=3%%%                               % the number of the states of the output y  
mu=3%%%                               % the number of the states of the  input u
m=6%%%                                % the number of discretization intervals
F=3%%%                                % the number of filters used
lambda=1%%%                           % forgetting factor

%simulation

T=5%%%                                % 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
T0=3%%%                               % memory length of the system
Y(1:T0)=0.4*ones(1,T0)%%%             % initial outputs
U(1:T0)=5*ones(1,T0)%%%               % initial inputs
U(T0+1)=0.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
fy=Y%%%                               %filtered continuous outputs
Ify=Y%%%                              %ideal filtered continuous outputs
fu=U%%%                               %filtered continuous inputs
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=1%%%                               % 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=10%%%                          % boundaries of continuous output
Ylower=0%%%
Uupper=5%%%                           % boundaries of continuous output
Ulower=-5%%%
                                    % set point of scaled continuous output
IY=(IY-(Yupper+Ylower)/2)/((Yupper-Ylower)/2)%%% 
                                    % set point of scaled continuous input
IU=(IU-(Uupper+Ulower)/2)/((Uupper-Ulower)/2)%%%  
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
     Y1(t)=(Y(t)-(Yupper+Ylower)/2)/((Yupper-Ylower)/2)%%%  % scaling
     U1(t)=(U(t)-(Uupper+Ulower)/2)/((Uupper-Ulower)/2)%%%  % scaling
     for i=1:F
       X(i)=Y1(t-i+1)%%%              % normalized state 
       X(i+F)=U1(t-i+1)%%%
       IX(i)=IY%%%                    % ideal discrete state
       IX(i+F)=IU%%%
     end
%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)')%%%
%filtering     
     for fil=1:F                           %filter loop
       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
%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  
     v=mweig(pred,m,lambda,F,v0,v)%%%
     mp=mpool(contr,m,F,v)%%%
%     U(t+1)=0%%%
     U(t+1)=mcontin1(mp,m,Ulower,Uupper)%%%
end

%drawing
hold off
plot(Y)%%%
hold on
plot(U,'r')%%%