% Program for simulating and estimating parametric dynamic factor models
% from dinfactest6 ..... used fot N=4 series
%  Application in Camacho, Poncela, Quiros
%  Estimate AR(2) common AR(2) specific 1 common factor, test of AR(1) vs. AR(2) in common factor
%  The drift parameters are estimated as sample means and do no enter the likelihood
%
% DGP and MODEL specification
%

% randn('state',100)
% fid=fopen('MCEST','w');

N=4;                            %number of series
T=526;
T=524;
IN=50;                          %IN+1=pre-sample length for simulation
NREP=1;                      %Number of Monte Carlo replications (1 at the moment)

PHIS=zeros(N,3);
Vi=zeros(N,1);

%DGP and Model one common factor

RHO1=.768/.84;                                %1st Autoregressive parameters in common factors
% RHO2=-.12/.84 ;   %2nd Autoregressive parameters in common factors
RHO2=0;
RHOS1=[0.5 0.5 0.5 0.5 ];             %1st Autoregressive parameters in specific factors
%RHOS1=[1 1 1]*.495/.95;
%RHOS2=[1 1 1]*(-.04/.95);         ;                %2nd Autoregressive parameters in specific factors
RHOS2=[0 0 0 0 ];
PI=[.1 .1 .1 .1 ]';                      %Vector of constant terms
C=[.7 .8 .9 1.0 ]';                            %Loadings
GAM=[0.51 0.36 0.19 0.32 ]';                     %Idiosyncratic variances
%Rescaling
SIG=C*C'+diag(GAM);
C=C./sqrt(diag(SIG));
GAM=GAM./diag(SIG);



% %Compute signal to noise ratio
% XPI=0:0.001:pi;
% LXPI=length(XPI);
% filtu1=[1+RHOS1(1)^2 -2*RHOS1(1)^2];
% filtu2=[1+RHOS1(2)^2 -2*RHOS1(2)^2];
% filtu3=[1+RHOS1(3)^2 -2*RHOS1(3)^2];
% filtu4=[1+RHOS1(4)^2 -2*RHOS1(4)^2];
% filtu5=[1+RHOS1(5)^2 -2*RHOS1(5)^2];
% 
% 
% for i=1:N,
%     Vu(i)=GAM(i)*(1 - RHOS1(i)^2);
% end
% Vf=1-RHO1(1)^2;
% GUINV=zeros(N,N,LXPI);
% GXINV=zeros(LXPI,1);
% GSIGN=zeros(LXPI,1);
% GUINVD=zeros(N,LXPI);
% for i=1:LXPI
%     GUINV(:,:,i)=diag([(filtu1(1)+filtu1(2)*cos(XPI(i)))/Vu(1) ...
%         (filtu2(1)+filtu2(2)*cos(XPI(i)))/Vu(2) ...
%         (filtu3(1)+filtu3(2)*cos(XPI(i)))/Vu(3) ...
%         (filtu4(1)+filtu4(2)*cos(XPI(i)))/Vu(4) ...
%         (filtu5(1)+filtu5(2)*cos(XPI(i)))/Vu(5)]);
%     GXINV(i)=(1+RHO1(1)^2-2*RHO1(1)*cos(XPI(i)))/Vf;
%     GSIGN(i)=C'*GUINV(:,:,i)*C+GXINV(i);
%     GUINVD(:,i)=diag(GUINV(:,:,i));
% end
% 
% %sum(GUINVD/GSIGN)
% 
% 
% signcom=1-sum(1./GSIGN)/LXPI;
% 
% signspec=1-C.^2.*(sum(GUINVD'./repmat(GSIGN,1,5))/LXPI)';
% 
% clear XPI LXPI filtu1 filtu2 filtu3 Vu GUINV GXINV GSIGN GUINVD
% 
% 


% Variables inizialization
N1=1+N;
Y=zeros(IN+1+T,N);                           %Data
X=zeros(IN+1+T,1);                           %Common factors
F=zeros(IN+1+T,1);                           %Common factors noise
U=zeros(IN+1+T,N);                           %Specific factors
V=zeros(IN+1+T,N);                           %Specific factors noise
Iyy=zeros(N,N,T);                            %Periodogram
In=sparse(eye(N));
FILT=zeros(N,N,T);
GU=zeros(N,N,T);
GY=zeros(N,N,T);
GYinv=zeros(N,N,T);
GVARS=zeros(N,N,T);
D=duplicmat(N);
Dinv=D*inv(D'*D);
In2Knn=2*Dinv*D';
En=matent(N);
Knn=commatrix(N,N);
DvGdRHO1=zeros(N*N,T);
DvGdRHO2=zeros(N*N,T);
DvGdRHO3m=zeros(N*N,T);;
DvGdC=zeros(N*N,N,T);
DvGdC1m=zeros(N*N,N,T);
DvGdG=zeros(N*N,N,T);
DvGdT1=zeros(N*N,N,T);
DvGdT2=zeros(N*N,N,T);
DvGdT3m=zeros(N*N,N,T);
DvGdPSI=zeros(N*N,N*N,T);
DFDP=zeros(N,T);

XPARMC=zeros(NREP,4*N+2);
XINFMC=zeros(NREP,4*N+2);
EFLAG=zeros(NREP,1);
xculo=zeros(NREP,1);
meang=zeros(NREP,1);
test =zeros(NREP,1);
tests=zeros(NREP,1);

% testf1a=zeros(NREP,1);
% testsa=zeros(NREP,1);
% testja=zeros(NREP,1);
testf1m=zeros(NREP,1);
testf2m=zeros(NREP,1);
testsm=zeros(NREP,1);
testjm=zeros(NREP,1);



IESTIM=1;

for irep=1:NREP
    disp(['REPLICATION  =' blanks(2) num2str(irep)])

    %**************************************************************************
    % Simulate data

    F=mvnrnd(0,1-RHO1^2-RHO2^2,IN+1+T);
    X=filter(1,[1 -RHO1 -RHO2],F);
    V=mvnrnd(zeros(N,1),(In-diag(RHOS1).^2-diag(RHOS2).^2).*diag(GAM),IN+1+T);
    for in=1:N
        U(:,in)=filter([1],[1 -RHOS1(in) -RHOS2(in) ],V(:,in));
    end
    Y=X*C'+U+repmat(PI',IN+1+T,1);
    Yobs=Y(IN+2:IN+T+1,:);

    
% IN CASE YOU WANT TO RUN A MONTE CARLO APPLICATION PLEASE COMMENT UNTIL ROW: 'END OF DATA LOADING'    
    
%     load DATA/Gaby4.txt
%     
%     GRATES=(log(Gaby4(2:end,:))-log(Gaby4(1:end-1,:)))*100;
%     
% %    Yobs=[GRATES(:,1:2) GRATES(:,5) GRATES(:,3:4)];
% %    Yobs=Yobs(1:132,:);
%     
% %     Yobs(1:132,1:2)=Yobs(1:132,1:2)/100;
% %     Yobs(1:132,5)=Yobs(1:132,5)*100;
%     
%        Yobs=GRATES;
%         Yobs=[GRATES(:,1) GRATES(:,3) GRATES(:,4) GRATES(:,2) ];
%  
%         Yobsold=Yobs;
        
      load CPPdata.txt  
        
       Yobsrow=Gaby4new; 
       
       for i=1:N,
           Yobs(:,i)=(Yobsrow(:,i)-mean(Yobsrow(:,i)))/std(Yobsrow(:,i));
%            Yobsold(:,i)=(Yobsold(:,i)-mean(Yobsold(:,i)))/std(Yobsold(:,i));

       end
       
       Yobs=Yobs(3:end,:);
       
       
%END OF DATA LOADING       
       
%**************************************************************************
    % Estimation
    % First Remove mean of Yobs

    PIhat=mean(Yobs)';
    Yobs=Yobs-repmat(PIhat',T,1);

    % Compute Periodogram ************************************************
    Iyy=zeros(N,N,T);
    y=fft(Yobs);
    for i=1:T,
        Iyy(:,:,i)=y(i,:).'*conj(y(i,:))/T;
    end
%     Iyy=real(Iyy);
    Iyy(:,:,1)=mean(Yobs)'*mean(Yobs)*T;    


    GU=zeros(N,N,T);
    GX=zeros(T,1);
    GY=zeros(N,N,T);
    lambda=2*pi*[0:T-1]/T;
    coslambda=cos(lambda);
    cos2lambda=cos(2*lambda);
    cos3lambda=cos(3*lambda);
    cos4lambda=cos(4*lambda);
    elambda=coslambda -sqrt(-1)*sin(lambda);
    elambda2=cos2lambda -sqrt(-1)*sin(2*lambda);
    TR=zeros(T,1);
    LDET=zeros(T,1);


%     
%     % **********************************************************************



   X0=zeros(1,18);
   X0(1:4)=[ .5   .5   .5  .5 ]
   X0(5:8)=[.3 .23 .8 .5];
   X0(9:10)=[.4 .2 ];
   X0(11:18)=[0 0 0 0 0 0 0 0];

 X0(1:4)=[ .69   .5   .28  .45 ]
   X0(5:8)=[.26 .24 .85 .59];
   X0(9:10)=[.47 .22 ];
   X0(11:18)=[-.26 .23 -.2  -.36  -.21  .53 -.05 -.15];
  
   X0
   pause
   
    % Estimate when IESTIM=1
    if (IESTIM==1)
%         [x fval exitflag]=findMLGRAN4(X0,N,N1,T,Iyy,M,GX,GU,GY,GYinv,TR,LDET, ...
%             DvGdRHO1,DvGdC,DvGdG,DvGdT1,D,Dinv,In2Knn,En,coslambda,cos2lambda,cos3lambda,cos4lambda);
       [x fval exitflag output lambda g h]=findMLGRAN41(X0,N,N1,T,Iyy,GX,GU,GY,GYinv,TR,LDET, ...
            DvGdRHO1,DvGdC,DvGdG,DvGdT1,D,Dinv,In2Knn,En,coslambda,cos2lambda,cos3lambda,cos4lambda);
        
        fvalmc(irep)=fval;
%         XPARMC(irep,:)=[PIhat' x];
        EFLAG(irep)=exitflag;
    else
        x=X0;
%         x=XPARMCOLD(irep,6:end);
    end

%     fprintf(fid,'%12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f \n',XPARMC(irep,:));
    %***************************************************************************

    
% 
%     % Compute tests
% 
% 
%     %Fill parameters vectors
       Chat=x(1:N)';
        GAMhat=(x(N1:2*N)');
        RHO1hat=x(2*N+1);
        RHO2hat=x(2*N+2);
        RHOS1hat=x(2*N+2+1:3*N+2);
        RHOS2hat=x(3*N+2+1:4*N+2);
        PHI=[1 -RHO1hat -RHO2hat];
        Vf=1;
        Vi=zeros(N,1);
        for i=1:N,
            PHIS(i,:)=[1 -RHOS1hat(i) -RHOS2hat(i)];
            Vi(i)=GAMhat(i);
        end

% 
%     %Compute gradient and infmat at estimated values

        GXdenom=coj(PHI,PHI);
        GXdenom2=multh(GXdenom,GXdenom);
        GU1denom=coj(PHIS(1,:),PHIS(1,:));
        GU2denom=coj(PHIS(2,:),PHIS(2,:));
        GU3denom=coj(PHIS(3,:),PHIS(3,:));
        GU4denom=coj(PHIS(4,:),PHIS(4,:));
        h1=[-2*RHO1hat(1)    -2*(RHO2hat(1)-1)];
        h2=[-2*RHO2hat(1)    -2*RHO1hat(1)      2];
        k11=[-2*RHOS1hat(1) -2*(RHOS2hat(1)-1)];
        k12=[-2*RHOS1hat(2) -2*(RHOS2hat(2)-1)];
        k13=[-2*RHOS1hat(3) -2*(RHOS2hat(3)-1)];
        k14=[-2*RHOS1hat(4) -2*(RHOS2hat(4)-1)];
        k21=[-2*RHOS2hat(1)    -2*RHOS1hat(1)      2];
        k22=[-2*RHOS2hat(2)    -2*RHOS1hat(2)      2];
        k23=[-2*RHOS2hat(3)    -2*RHOS1hat(3)      2];
        k24=[-2*RHOS2hat(4)    -2*RHOS1hat(4)      2];
        CkC=kron(Chat,Chat);
        CC=Chat*Chat';

        GX=Vf./(GXdenom(1)+GXdenom(2)*coslambda+GXdenom(3)*cos2lambda)';
        GUU=repmat(Vi,1,T)./[GU1denom(1)+GU1denom(2)*coslambda+GU1denom(3)*cos2lambda; ...
                             GU2denom(1)+GU2denom(2)*coslambda+GU2denom(3)*cos2lambda; ...
                             GU3denom(1)+GU3denom(2)*coslambda+GU3denom(3)*cos2lambda; ...
                             GU4denom(1)+GU4denom(2)*coslambda+GU4denom(3)*cos2lambda];
        GU=reshape(En'*GUU,N,N,T);





       GY=reshape((repmat(CC,T,1).*kron(GX,ones(N,N)))',N,N,T)+GU;
%     % Gradient and Infmat

        INFMATm=zeros(5*N+3+N+N*N+N,5*N+3+N+N*N+N);
        DFDPm=zeros(5*N+3+N+N*N+N,1);


        for i=1:T
            GYinv(:,:,i)=inv(GY(:,:,i));
            M=kron(GYinv(:,:,i),GYinv(:,:,i).');
            GXdenom2l=GXdenom2(1)+GXdenom2(2)*coslambda(i)+GXdenom2(3)*cos2lambda(i)+GXdenom2(4)*cos3lambda(i) ...
                +GXdenom2(5)*cos4lambda(i);
            h1l=h1(1)+h1(2)*coslambda(i);
            h2l=h2(1)+h2(2)*coslambda(i)+h2(3)*cos2lambda(i);
            Alam=diag(ones(1,N)-RHOS1hat*elambda(i)-RHOS2hat*elambda2(i));
            DvGdRHO1(:,i)=CkC*((Vf*h1l./GXdenom2l));
            DvGdRHO2(:,i)=CkC*((Vf*h2l./GXdenom2l));
            DvGdRHO3m(:,i)=CkC*2*coslambda(i)*GX(i);
            DvGdC(:,:,i)=(In2Knn)*kron(eye(N),Chat*GX(i));
%             DvGdC1m(:,:,i)=repmat(Chat',N^2,1).*(kron(eye(N),conj(elambda(i))*Chat*GX(i))+kron(elambda(i)*Chat*GX(i),eye(N) ));
          DvGdC1m(:,:,i)=(kron(eye(N),conj(elambda(i))*Chat*GX(i))+kron(elambda(i)*Chat*GX(i),eye(N) ));
%             DvGdC1LEADm=repmat(Chat',N^2,1).*(kron(eye(N),elambda(i)*Chat*GX(i))+kron(conj(elambda(i))*Chat*GX(i),eye(N) ));
           DvGdC1LEADm=(kron(eye(N),elambda(i)*Chat*GX(i))+kron(conj(elambda(i))*Chat*GX(i),eye(N) ));
            DvGdG(:,:,i)=En'*diag((diag(GU(:,:,i))./Vi));
            k11l=k11(1)+k11(2)*coslambda(i);
            k12l=k12(1)+k12(2)*coslambda(i);
            k13l=k13(1)+k13(2)*coslambda(i);
            k14l=k14(1)+k14(2)*coslambda(i);
            k1l=[k11l k12l k13l k14l]';
            k21l=k21(1)+k21(2)*coslambda(i)+k21(3)*cos2lambda(i);
            k22l=k22(1)+k22(2)*coslambda(i)+k22(3)*cos2lambda(i);
            k23l=k23(1)+k23(2)*coslambda(i)+k23(3)*cos2lambda(i);
            k24l=k24(1)+k24(2)*coslambda(i)+k24(3)*cos2lambda(i);
            k2l=[k21l k22l k23l k24l]';
            DvGdT1(:,:,i)=En'*diag((k1l.*diag(GU(:,:,i).^2)./Vi));
            DvGdT2(:,:,i)=En'*diag((k2l.*diag(GU(:,:,i).^2)./Vi));
            DvGdT3m(:,:,i)=En'*diag((2*coslambda(i)*diag(GU(:,:,i))));
            DvGdPSI(:,:,i)=-Knn*kron(inv(Alam),GY(:,:,i).'*Alam)*elambda(i)-kron(conj(inv(Alam)),GY(:,:,i)*conj(Alam))*conj(elambda(i));
            DGDPm=[DvGdC(:,:,i) DvGdG(:,:,i)  DvGdRHO1(:,i) DvGdRHO2(:,i) DvGdT1(:,:,i) DvGdT2(:,:,i) ... 
                      DvGdRHO3m(:,i)  DvGdT3m(:,:,i) DvGdC1m(:,:,i) DvGdPSI(:,:,i)  DvGdC1LEADm];
            INFMATm=INFMATm+DGDPm.'*M*conj(DGDPm);
            DFDPm=DFDPm+DGDPm.'*vec(GYinv(:,:,i).'*Iyy(:,:,i).'*GYinv(:,:,i).'-GYinv(:,:,i).');
        end
            %
     DFDPm=real(DFDPm)/T/2;
     INFMATm=real(INFMATm)/T/2;
     INFMATinv=inv(INFMATm(1:23,1:23));
     INFMATinv1=inv(INFMATm(1:19,1:19));
     INFMATinvs=inv(INFMATm([1:18 20:23] ,[1:18 20:23]));
     INFMATinvC1=inv(INFMATm([1:18 24:27] ,[1:18 24:27]));
     INFMATinvsC1=inv(INFMATm([1:18 20:27] ,[1:18 20:27]));
     INFMATinvPSI=inv(INFMATm([1:18 28:43] ,[1:18 28:43]));

     %     XINFMC(irep,:)=diag(INFMATINV)';
%     
%     %Additive tests
% 
% %     testf1a(irep)=T*DFDPa(3*N+2)^2*INFMATai(3*N+2,3*N+2);
% %     testsa(irep)=T*DFDPa(3*N+3:3*N+2+N)'*INFMATai(3*N+3:3*N+2+N,3*N+3:3*N+2+N)*DFDPa(3*N+3:3*N+2+N);
% %     testja(irep)=T*DFDPa(3*N+2:3*N+2+N)'*INFMATai(3*N+2:3*N+2+N,3*N+2:3*N+2+N)*DFDPa(3*N+2:3*N+2+N);
% 
% 
%     %Multiplicative tests
% 
     testf1m(irep)=T*DFDPm(4*N+3)^2*INFMATinv1(4*N+3,4*N+3);
%     testf2m(irep)=T*DFDPm([3*N+2 4*N+3])'*INFMATmi2([3*N+2 4*N+3],[3*N+2 4*N+3])*DFDPm([3*N+2 4*N+3]);
%     testsm(irep)=T*DFDPm(3*N+3:4*N+2)'*INFMATmi1(3*N+3:4*N+2,3*N+3:4*N+2)*DFDPm(3*N+3:4*N+2);
     testsm(irep)=T*DFDPm(4*N+4 :5*N+3)'*INFMATinvs(19 :22,19 :22)*DFDPm(4*N+4 :5*N+3);
     testjm(irep)=T*DFDPm(4*N+3 :5*N+3)'*INFMATinv(4*N+3 :5*N+3,4*N+3 :5*N+3)*DFDPm(4*N+3 :5*N+3);
     testC1m(irep)=T*DFDPm(24 :27)'*INFMATinvC1(19 :22,19 :22)*DFDPm(24 :27);
     testsC1m(irep)=T*DFDPm(20 :27)'*INFMATinvsC1(19 :26,19 :26)*DFDPm(20 :27);
     testRm(irep)=T*DFDPm(28 :43)'*INFMATinvPSI(19 :34,19 :34)*DFDPm(28 :43);
     INFMATinv1=inv(INFMATm([1:18 44:47],[1:18 44:47]));
     testC1LEADm(irep)=T*DFDPm(44 :47)'*INFMATinv1(19 :22,19 :22)*DFDPm(44 :47);
% 
% 
% 

      INFMATinv1=inv(INFMATm([1:18 20],[1:18 20]));
      tests1=T*DFDPm(20)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 21],[1:18 21]));
      tests2=T*DFDPm(21)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 22],[1:18 22]));
      tests3=T*DFDPm(22)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 23],[1:18 23]));
      tests4=T*DFDPm(23)^2*INFMATinv1(19,19);

      INFMATinv1=inv(INFMATm([1:18 24],[1:18 24]));
      testC1=T*DFDPm(24)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 25],[1:18 25]));
      testC2=T*DFDPm(25)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 26],[1:18 26]));
      testC3=T*DFDPm(26)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 27],[1:18 27]));
      testC4=T*DFDPm(27)^2*INFMATinv1(19,19);
      
      INFMATinv1=inv(INFMATm([1:18 44],[1:18 44]));
      testC1F=T*DFDPm(44)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 45],[1:18 45]));
      testC2F=T*DFDPm(45)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 46],[1:18 46]));
      testC3F=T*DFDPm(46)^2*INFMATinv1(19,19);
      INFMATinv1=inv(INFMATm([1:18 47],[1:18 47]));
      testC4F=T*DFDPm(47)^2*INFMATinv1(19,19);
      
%Hosking 
INFMATinv=inv(INFMATm([1:18 28] ,[1:18 28]));
testR1(irep)=T*DFDPm(28)'*INFMATinv(19,19 )*DFDPm(28);

INFMATinv=inv(INFMATm([1:18 33] ,[1:18 33]));
testR2(irep)=T*DFDPm(33)'*INFMATinv(19,19 )*DFDPm(33);

INFMATinv=inv(INFMATm([1:18 38] ,[1:18 38]));
testR3(irep)=T*DFDPm(38)'*INFMATinv(19,19 )*DFDPm(38);

INFMATinv=inv(INFMATm([1:18 43] ,[1:18 43]));
testR4(irep)=T*DFDPm(43)'*INFMATinv(19,19 )*DFDPm(43);



end
[[1:length(x)]' x' sqrt(diag(inv(h)))]
testf1m
testsm
[tests1 tests2 tests3 tests4]
testCm
[testC1 testC2 testC3 testC4]
testCFm
[testC1F testC2F testC3F testC4F]
testRm
[testR1 testR2 testR3 testR4]

%Filtering and Smoothing


%System Matrices
Zidio=[1 0 0 0 0 0 0 0; 0 0 1 0 0 0 0 0; 0 0 0 0 1 0 0 0; 0 0 0 0 0 0 1 0];
Z=[Chat zeros(N,1) Zidio];
TT=zeros(10,10);
    TT(1,1:2)=[RHO1hat RHO2hat];
    for i=1:5
        TT(2*i,2*i-1)=1;
    end
    for i=1:4
        TT(2*i+1,2*i+1:2*i+2)=[RHOS1hat(i) RHOS2hat(i)];
    end

    Q=zeros(10,10);
    Q(1,1)=1;
     for i=1:4
        Q(2*i+1,2*i+1)=GAMhat(i);
     end

%Initialize state mean and covariance matrices
Att1=zeros(2*N1,T);                        %Kalman prediccion
Ptt1=zeros(2*N1,2*N1,T);                    %Variance of Kalman prediccion
Att=zeros(2*N1,T);                         %Kalman updating
Ptt=zeros(2*N1,2*N1,T);                     %Variance of Kalman updating
    
     
     
%Kalman Filter
%initialize as in CPP
Att1(:,1)=0;
Ptt1(:,:,1)=TT*eye(10)*TT'+Q;
Sigmat=Z*Ptt1(:,:,1)*Z';
Sigmatinv=inv(Sigmat);
mut=Z*Att1(:,1);
Att(:,1)=Att1(:,1)+Ptt1(:,:,1)*Z'*Sigmatinv*(Yobs(1,:)'-Z*Att1(:,1));
Ptt(:,:,1)=Ptt1(:,:,1)-Ptt1(:,:,1)*Z'*Sigmatinv*Z*Ptt1(:,:,1);

% Recursion
for it=2:T
    %predict
    Att1(:,it)=TT*Att(:,it-1);
    Ptt1(:,:,it)=TT*Ptt(:,:,it-1)*TT'+Q;
    Sigmat=Z*Ptt1(:,:,it)*Z';
    Sigmatinv=inv(Sigmat);
    mut=Z*Att1(:,it);
    ZpS=Z'*Sigmatinv;
    ZpSZ=ZpS*Z;
    PZpS=Ptt1(:,:,it)*ZpS;
    ZpSZP=ZpSZ*Ptt1(:,:,it);
    %update
    Att(:,it)=Att1(:,it)+PZpS*(Yobs(it,:)'-mut);
    Ptt(:,:,it)=Ptt1(:,:,it)-Ptt1(:,:,it)*ZpSZP;
end

    %Fixed-Interval smoothing

    AtT=zeros(10,T);
    PtT=zeros(10,10,T);
    AtT(:,T)=Att(:,T);
    PtT(:,:,T)=Ptt(:,:,T);
    for it=T-1:-1:1,
        Pstar=Ptt(:,:,it)*TT'*pinv(Ptt1(:,:,it+1));
        AtT(:,it)=Att(:,it)+Pstar*(AtT(:,it+1)-TT*Att(:,it));
        PtT(:,:,it)=Ptt(:,:,it)+Pstar*(PtT(:,:,it+1)-Ptt1(:,:,it+1))*Pstar';
    end

% save culo200af


% fclose(fid)
