
function  [stateSMOOTH,obsSMOOTH,eSMOOTH,MEASerrSMOOTH,STDstateSMOOTH]= ...
    SMOOTHER_no_kalman_RK(F1,F2,COVe,Hobs,RHOMeasErr,CovMeasErr,DATA);
                       
                          
if nargin~=7; error('need 7 arguments'); end;

% This proc constructs smoothed estimates of states in linearized DSGE model
% August 27, 2011
% Written by Robert Kollmann (ECARES, Universite Libre de Bruxelles and CEPR), 
% robert_kollmann@yahoo.com
% THIS PROC ALLOWS TO USE DATASETS WITH MISSING OBSERVATION !

% Assume state-space model: 
% s(t)=F1*s(t-1)+F2*e(t)   t=1,..,T;   COVe=E(e(t)*e(t)')
% y(t)=Hobs*y(t)+w(t)  
% w(t)=RHOMeasErr*w(t-1)+mu(t),  CovMeasErr=Cov(mu(t))
% s(t): state;  y(t): observables;  w(t): measurement error
% v(t) & mu(t): normal, independent
% DATA: a T x(number of observables) matrix of empirical measures of the observables 
% Column i pf DATA is observed time series for variable 'i'; missing observations have to be
% shown as 'NaN' entries in 'DATA' matrix

%OUTPUT: 
% stateSMOOTH=E(S(t)|I(T))     for t=1,..,T  Smoothed STATE variables 
% obsSMOOTH=E(y|I(T))       for t=1,..,T  Smoothed observable 
% eSMOOTH=E(e(t)|I(t))         for t=2,..,T  Smoothed 'e' (structural innovation)
% MEASerrSMOOTH=E(w(t)|I(t))   for t=1,..,T  Smoothed 'w' (measurement error) 
% STDstateSMOOTH=Std({E(S(t)|I(T))})  for t=1,..,T  Standard deviation of smoothed estimates of states
% I(t)=(y(1),y(2),..,y(t): history of observables up to date t  


if rows(Hobs)~=cols(DATA); error('AAA'); end;
if rows(COVe)~=cols(COVe); error('AAA1'); end;
if rows(CovMeasErr)~=cols(CovMeasErr); error('AAA2'); end;
if rows(F1)~=cols(F1); error('AAA3'); end;
if cols(F2)~=cols(COVe); error('AAA5'); end;
if rows(F1)~=rows(F2); error('AAA6'); end;



T=rows(DATA); 
Ns=cols(F1);     % number of state variables
Nv=cols(F2);     % number of innovations to state variables
Nobs=cols(DATA); % number of observed variables


%.....................................................................
%  Stack states and measurement error: z(t)=[s(t);w(t)];
%  z(t)=LAM1_A*z(t-1)+LAM2_A*[e(t);mu(t)]
%  y(t)=H_KAL_A*Z(t);  
%  See: S.Schmitt-Grohe & M.Uribe, 2011, Economics Letters, Vol. 109, pp.142-143 

LAM1_A=zeros(Ns+Nobs,Ns+Nobs);
LAM1_A(1:Ns,1:Ns)=F1; 
LAM1_A(Ns+1:end,Ns+1:end)=RHOMeasErr; 

H_KAL_A=[Hobs eye(rows(Hobs))];

LAM2_A=zeros(Ns+rows(CovMeasErr),cols(COVe)+rows(CovMeasErr));
LAM2_A(1:Ns,1:cols(COVe))=F2;
LAM2_A(Ns+1:end,cols(COVe)+1:end)=eye(rows(CovMeasErr));

COVew=zeros(rows(COVe)+rows(CovMeasErr),rows(COVe)+rows(CovMeasErr));
COVew(1:rows(COVe),1:rows(COVe))=COVe;
COVew(rows(COVe)+1:end,rows(COVe)+1:end)=CovMeasErr;
% COVew=Cov([e(t); w(t)])

COVv=F2*COVe*F2';
V_A=zeros(Ns+Nobs,Ns+Nobs);
V_A(1:Ns,1:Ns)=COVv; 
V_A(Ns+1:end,Ns+1:end)=CovMeasErr;



% ............................................................
% Use doubling algorithm to compute Cov(Z(t))
SS=ardouble(LAM1_A,V_A);
[V,D]=eig(SS);

% .......................................................................
% .......................................................................
% Z=[z(1);z(2);...;z(T)];
% If  SS=Cov(Z) is singular, then reduce the dimension of the system to obtain
% non-singular system

% Define state variables h(t) [column vector] that spans z(t):  
% z(t)=Gwh*h(t),  where h(t) is non-singular  
% Define state-space form: 
% h(t)=LAM1_R*h(t-1)+LAM2_R*[e(t);mu(t)]
% y(t)=H_KAL_R*h(t): observable as function of reduced state vector

if min(min(abs(D)))<1e-8;   % if SS is singular
    
V1=V(:,max(abs(D))>1e-8);
D1=D(max(abs(D))>1e-8,max(abs(D))>1e-8);

Gwh=SS*V1*diag(1./diag(D1));
% NB Gwh'*Gwh=I
if maxaa(Gwh'*Gwh-eye(cols(Gwh)))>1e-8; error('BBB1'); end;


Sh=V1'*SS*V1;     % Sh=E(h*h')
if maxaa(Gwh*Sh*Gwh'-SS)>1e-7; error('AAA3'); end;
LAM1_R=Gwh'*LAM1_A*Gwh;
LAM2_R=Gwh'*LAM2_A;
H_KAL_R=H_KAL_A*Gwh;
P0=Sh;  
SW_R=Sh;
end;   % end  if min(abs(D))<1e-8; 

% .......................................................................
% .......................................................................
if min(min(abs(D)))>=1e-8;  
% if SS is not singular, then no need to find reduced system
LAM1_R=LAM1_A;
LAM2_R=LAM2_A;
H_KAL_R=H_KAL_A;
P0=SS; 
SW_R=SS;
Gwh=eye(rows(LAM1_A));
end;   

% .......................................................................
% .......................................................................


% Z=[z(1);z(2);...;z(T)]: stacked (column) vector of original states and measurement errors 
% H=[h(1);h(2);...;h(T)]: stacked (column) vector with (reduced) states for t=1,..,T
% Y=[y(1);y(2);...;y(T)]: stacked (column) vector of observables

% ========== computing E(H*H') ===========
% See Schmitt-Grohe and Uribe (2011) code
HX = SW_R*H_KAL_R';
A=H_KAL_R*HX/2;

for i=1:T
L((i-1)*Nobs+1:i*Nobs,1:size(A,2))= A;
HX = LAM1_R*HX;
A = [H_KAL_R*HX A];
end

LL=L+L';   % E(Y*Y')  cov of stacked observables


% ========== computing E(H*Y') ===========
HZ = SW_R;
AZ=HZ/2;

kZ=size(AZ,2);

for i=1:T
LZ((i-1)*kZ+1:i*kZ,1:size(AZ,2))= AZ;
HZ = LAM1_R*HZ;
AZ = [HZ AZ];
end

LZLZ=LZ+LZ';  % E(H*H');

EHY=LZLZ*kron(eye(T),H_KAL_R)';  % E(H*Y');


% =========================================
% SELECTION MATRIX (to pick just observables for which empirical counterpart is NOT missing)
% WW: selection matrix (it selects the observables that are not missing)

DnonMISS=isnan(DATA)==0; DnonMISS=+DnonMISS;
% This converts logical array to numerical array

if rows(DnonMISS)~=rows(DATA); error('A1'); end;
if cols(DnonMISS)~=cols(DATA); error('A2'); end;
if maxaa((DnonMISS==1)+(DnonMISS==0)-ones(rows(DnonMISS),cols(DnonMISS)))>1e-8; error('A3'); end;
if min(sum(DnonMISS'))<=1e-8; error('A4'); end;

DnonMISS1=DnonMISS'; DnonMISS2=DnonMISS1(:); WW1=diag(DnonMISS2); WW=WW1(sum(WW1')>0,:);
WW=+WW; % this ensures that WW is not 'logical'

% .............................
DATAp=DATA;
DATAp(isnan(DATAp))=0;

Ya=DATAp'; Y=WW*Ya(:);
% .............................


MINV=inv(WW*LL*WW');

hSMOOTH=EHY*WW'*MINV*Y;
% smoothed state vector H

if rows(hSMOOTH)~=rows(LAM1_R)*T; error('QQQ'); end;

hSMOOTH_a=reshape(hSMOOTH,rows(LAM1_R),T)';
xSMOOTH=hSMOOTH_a*Gwh';  % smoothed original states


Vsm=EHY*WW'*MINV*WW*EHY'; 
% covariance matrix of smoothed H=[h(1);h(2);..,h(T)]



Nh=cols(Gwh);

if rows(Vsm)~=Nh*T; error('PP'); end;

% Standard deviations of smoothed series
vSMOOTH=[];
t=1; while t<=T;
vSMOOTH=[vSMOOTH; sqrt(diag(Gwh*Vsm(1+(t-1)*Nh:t*Nh,1+(t-1)*Nh:t*Nh)*Gwh'))'];
t=t+1; end;

PP=LAM2_R(:,diag(COVew)>0)'*LAM2_R(:,diag(COVew)>0);
if rank(PP)==rows(PP); 
ewSMOOTH=(hSMOOTH_a(2:end,:)*LAM2_R(:,diag(COVew)>0)-hSMOOTH_a(1:end-1,:)*LAM1_R'*LAM2_R(:,diag(COVew)>0))*inv(PP);
end;

PPx=LAM2_A(:,diag(COVew)>0)'*LAM2_A(:,diag(COVew)>0);
ewSMOOTHx=(xSMOOTH(2:end,:)*LAM2_A(:,diag(COVew)>0)-xSMOOTH(1:end-1,:)*LAM1_A'*LAM2_A(:,diag(COVew)>0))*inv(PPx);

if rank(PP)<rows(PP); 
ewSMOOTH=ewSMOOTHx; 
end;



if maxaa(ewSMOOTH-ewSMOOTHx)>1e-3; save klm99; error('AA1'); end;

stateSMOOTH=xSMOOTH(:,1:rows(F1)); % smoothed states for t=1,2,..,T
obsSMOOTH=xSMOOTH*H_KAL_A';  % smoothed observables

if maxaa(DATA-obsSMOOTH)>1e-3; save klm99; error('AA11'); end;


eSMOOTH=zeros(rows(ewSMOOTH),cols(COVe));
eSMOOTH(:,diag(COVe)>0)=ewSMOOTH(:,1:sum(diag(COVe)>0)); 
% eSMOOTH: smoothed structural innovations for t=1,2,..,T


MEASerrSMOOTH=xSMOOTH(:,rows(F1)+1:end); 
%MEASerrSMOOTH: smoothed measurement errors for t=1,..,T

STDstateSMOOTH=vSMOOTH(:,1:rows(F1));  %standard devs of smoothed states

%save klm2011;




% ======================================================================
% Doubling algorithm
% Written by Robert Kollmann (ECARES, Universite Libre de Bruxelles and CEPR), 
% robert_kollmann@yahoo.com

function [s]=ardouble(r,d);
% March 16, 2001. Using doubling algorithm, this solves the following equation:
% s=r*s*r'+d

if rows(r)~=cols(r); return; end;
if rows(d)~=cols(d); return; end;
if rows(r)~=rows(d); return; end;


s0=d; r0=r;
j=1;  while j<=12;
%  s0=r0*s0*r0'+s0; r0=r0*r0;
% JANUARY 18, 2002
s0=r0*s0*permute(r0,[2 1])+s0; r0=r0*r0;
j=j+1; end;

% maxa(maxa(s0-r*s0*r'-d))

s=s0;


% ======================================================================

function wsl2 = cols(x);
wsl2=size(x,2);



function wsl1 = rows(x);
wsl1 = size(x,1);



function [z]=maxa(x);
z=max(max(abs(x)));


function [yxc]=maxaa(x);
yxc=maxa(maxa(maxa(maxa(maxa(maxa(maxa(x)))))));


