%[XI_tT,P_tT,XI_tt,P_tt,XI_ttm1,P_ttm1] = smoothing_kalman(Y,F,Q,H,R,S,A)
%produce smoothed series of the state xi in from the process
% x_t+1 = F x_t + v_t+1
% y_t = A'*s_t + H'x_t + w_t 
%var(v_t) = Q
%var(w_t) = R
% x_t is nx by 1
% F is nx by nx
% v_t is nx by 1
% y_t is ny by 1
% H' is ny by nx
% s_t is ns by 1
% A' is ny by ns
% w_t is ny by 1
% Q is nx by nx
% R is ny by ny
%Y is a T by ny matrix containing T observations of y_t
%S is a T by ns matrix containing T observations of s_t
%XI_tT is T-by-nx matrix, with row t given by the transpose of E_T(x_t) 
%XI_tt is T-by-nx matrix, with row t given by the transpose of E_t(x_t) 
%XI_ttm1 is T-by-nx matrix, with row t given by the transpose of E_t-1(x_t)
%P_tT is a nx-by-nx-by-T array with page (:,:,t) given by E_T[(x_t-E_T(tx_t))*(x_t-E_T(x_t))']
%P_tt is a nx-by-nx-by-T array with page (:,:,t) given by E_t[(x_t-E_t(tx_t))*(x_t-E_t(x_t))']
%P_ttm1 is a nx-by-nx-by-T array with page (:,:,t) given by E_t-1[(x_t-E_t-1(x_t))*(x_t-E_t-1(x_t))']
% See James D. Hamilton,  Time Series Analysis,  1994, chapter 13. 
%© Martín Uribe, 02-Dec-2017.
function [XI_tT,P_tT,XI_tt,P_tt,XI_ttm1,P_ttm1] = smoothing_kalman(Y,F,Q,H,R,S,A)

if nargin<7
S = 0*Y(:,1);
A = zeros(1,size(Y,2));
end
%Number of Observations
T = size(Y,1);

%Rows of transition matrix
r = size(F,1);
n = size(Y,2);

%Initial forecast
XIold = zeros(r,1);

%Doubling algorithm
F_old=F;
Q_old=Q;
Pold_old=eye(size(F));

XI_tT = zeros(T,r);
XI_tt = zeros(T,r);
XI_ttm1 = zeros(T,r);

P_tT = zeros(r,r,T);
P_tt = zeros(r,r,T);
P_ttm1 = zeros(r,r,T);

diferenz=0.1;
while diferenz>1e-25;
Pold =F_old*Pold_old*F_old' + Q_old;
diferenz = max(max(abs(Pold-Pold_old)));
Q_old=F_old*Q_old*F_old' + Q_old;
F_old = F_old * F_old;
Pold_old=Pold;
end    %while diferenz

Fprime = F';
Hprime = H';
Aprime = A';
Yprime = Y';
Sprime = S';

for t=1:T

XI_ttm1(t,:) = XIold';
P_ttm1(:,:,t) = Pold';

y = Yprime(:,t);
s = Sprime(:,t);
dev_y = y - Aprime*s -  Hprime*XIold;

var_y =Hprime*Pold*H+R;

ivar_y = var_y \ eye(n);

XI_tt(t,:) = transpose(XIold + Pold*H*ivar_y*dev_y);
P_tt(:,:,t) = Pold - Pold*H*ivar_y*Hprime*Pold;

XIold = F*XI_tt(t,:)';
 Pold = F*P_tt(:,:,t)*F' + Q;
end

XI_tT(T,:) =  XI_tt(T,:) ;
P_tT(:,:,T) = P_tt(:,:,T); 

for t=T-1:-1:1
J = P_tt(:,:,t)*Fprime*inv(P_ttm1(:,:,t+1));
XI_tT(t,:) = XI_tt(t,:) + transpose(J*(XI_tT(t+1,:) - XI_ttm1(t+1,:))');
P_tT(:,:,t) = P_tt(:,:,t) + J * (P_tT(:,:,t+1)-P_ttm1(:,:,t+1))*J';
end