%tethetast of simplified multi-modelling
%
%TEMPORARY CONCLUSIONS:
%
%  The following statements are valid for use of F'
%
% 1. Use of deviations improves the thing  ????
% 2. Controllers support each other
% 3. Control period serves as penalization but not sufficient
% 4. Estimates of standard deviation of noise are strange %%%%%%!!!!!!!
%    (permanently underestimated): possible explanation: 
%    wawelet coefficients are estimated
% 5. Gained controller is better than uncontrolled system
% 6. Gained controller is far from optimum value
% 7. Unclear whether weights in superposition of actions 
%    should be used: almost surely not
% 8. Model should contain absolute term! (shift may destroy the thing)
%

clear all
T=1000;                       % simulation length
                              % state space model
A=[0.95, -0.04, 0; 0.04,0.95,0;0, 0, -0.9]     %complex roots 
%A=[0.95, 0, 0 ;0, 0.95 0;0,0, 0.95]    %diagonal system
%A=[0.9, 0.1 -.6 ;0, 0.9 .5;0,0, 0.9]
%A=[0.9, 0.1 -.6 ;0, 0.9 .5;0,0,-0.9]   %oscilating system
B=[1-A(1,1);1-A(2,2);1-A(3,3)]
C=[1,1,1];
D=[0.0];
n=max(size(C));
% tripple time constant with a short sampling
%[ad,bd,C,D]=tf2ss([0,0,0,1],[1,3,3,1]) 
%[A,B]=c2d(ad,bd,.1)
[NUM,DEN] = ss2tf(A,B,C,D,1)  %transfer function
theta=[-DEN(2:(n+1)),NUM]     %regression parameters
y=zeros(T,1);                 %outputs 
y(1:n,1)=0.1*ones(n,1);       %initial conditions
u=zeros(T,1);                 %inputs   
m=2;
per=2^(m);                    %longest period
F=1/per*[ 1  1   1  1;        %transformation matrix
          1  1  -1 -1;
	  1 -1   1 -1;
	 -1  1   1 -1];
ye=zeros(T,per);              %filtered outputs
ue=zeros(T,per);              %filtered inputs
nest=1;                       %order of the simplified model
nn=nest*2+1;
ay=zeros(nest+1,per);         %coefficients of the output expansion
au=zeros(nest+1,per);         %coefficients of the input expansion       
thetaest=0.01*ones(nn,per);   %estimate of regression parameters
dia=1e0;                      %diagonal of covarince matrices
P=dia*[eye(nn),eye(nn),eye(nn),eye(nn)];
                              %covariance matrices 
y(1:per,1)=0.1*ones(per,1);   %initial conditions			      
uni=ones(per,1);
sig=0.1;                      %standard deviation of the noise 
rhat=sig^2*ones(per,1);       %estimates of the noise variance
kappa=ones(per,1);
us=zeros(per,1);              %array for input gained by synthesis
ee=zeros(per,1);              %array for prediction errors
q=5e-6;                       %input penalization
alpha=0.995;                  %forgetting factor
randn('seed',13)
for t=max(n,nest*per+1):T-per
%simulation
   y(t,1)=theta*[y(t-1:-1:t-n,1);u(t:-1:t-n,1)]+sig*randn;
%filtering and estimation 
   if rem(t,per)==0
%shift
   for i=nest+1:-1:2
       ay(i,:)=ay(i-1,:);
       au(i,:)=au(i-1,:);
   end
       for i=1:per
         ay(1,i)=F(i,:)*y(t-per+1:t,1);
	 au(1,i)=F(i,:)*u(t-per+1:t,1);
	 ye(t-per+1:t,i)=F(i,:)'*ay(1,i);
         ue(t-per+1:t,i)=F(i,:)'*au(1,i);
	 [thetaest(:,i),P(1:nn,(i-1)*nn+1:i*nn),rhat(i,1),kappa(i,1),ee(i,1)]=...
          rlsf(thetaest(:,i),P(1:nn,(i-1)*nn+1:i*nn),rhat(i,1),kappa(i,1),...
          ay(1,i),[ay(2:nest+1,i);au(:,i)],alpha);
%controller
         [us(i,1),cl]=cont(thetaest(:,i),q,ay(1,i),au(1,i));
      end	 
%compound controller      
           u(t+1:t+per,1)=F'*diag([1,1,1,1])*us;
%uncontrolled case for comparison	   
%	   u(t+1:t+per,1)=zeros(per,1);
end
end
plot(y)
pause
plot(ye(T-99:T,:))
pause 
plot(ue(T-99:T,:))
pause 
thetaest
rhat
pause
n=1
[a,b,c,d]=tf2ss(thetaest(2:3,4)',[1,-thetaest(1,1)])
n=2
[a,b,c,d]=tf2ss(thetaest(2:3,4)',[1,-thetaest(1,2)])
n=3
[a,b,c,d]=tf2ss(thetaest(2:3,4)',[1,-thetaest(1,3)])
n=4
[a,b,c,d]=tf2ss(thetaest(2:3,4)',[1,-thetaest(1,4)])

variance_y=y(T-99:T,1)'*y(T-99:T,1)/100
variance_u=u(T-99:T,1)'*u(T-99:T,1)/100