function [X,P,X0,P0,PP] = KalmanSm(A,Phi,Q,R,mu,S,XF,PF)
%function [X,P,X0,P0,PP] = KalmanSm(A,Phi,Q,R,mu,S,XF,PF)
%Kalman smoother

[p,n] = size(XF);
X = zeros(p,n);
P = zeros(p,p,n);
PP = zeros(p,p,n);

% Smoother
X(:,n) = XF(:,n);
P(:,:,n) = PF(:,:,n);
for t = n:-1:2
    P1 = Phi*PF(:,:,t-1)*Phi'+Q;
    J = PF(:,:,t-1)*Phi'*inv(P1);
    X(:,t-1) = XF(:,t-1) + J*(X(:,t)-Phi*XF(:,t-1));
    P(:,:,t-1) = PF(:,:,t-1) + J*(P(:,:,t)-P1)*J';
end
P1 = Phi*S*Phi'+Q;
J = S*Phi'*inv(P1);
X0 = mu + J*(X(:,1)-Phi*mu);
P0 = S + J*(P(:,:,1)-P1)*J';

% Lag-one covariance
P1 = Phi*PF(:,:,n-1)*Phi' + Q;
K = P1*A'*inv(A*P1*A'+R);
PP(:,:,n) = (eye(p)-K*A)*Phi*PF(:,:,n-1);
for t = n:-1:3
    J1 = PF(:,:,t-1)*Phi'*inv(Phi*PF(:,:,t-1)*Phi'+Q);
    J2 = PF(:,:,t-2)*Phi'*inv(Phi*PF(:,:,t-2)*Phi'+Q);
    PP(:,:,t-1) = PF(:,:,t-1)*J2' + J1*(PP(:,:,t)-Phi*PF(:,:,t-1))*J2';
end
J1 = PF(:,:,1)*Phi'*inv(Phi*PF(:,:,1)*Phi'+Q);
J2 = S*Phi'*inv(Phi*S*Phi'+Q);
PP(:,:,1) = PF(:,:,1)*J2' + J1*(PP(:,:,2)-Phi*PF(:,:,1))*J2';