%TF_SMOOTH  Two filter based Smoother
%
% Syntax:
%   [M,P] = TF_SMOOTH(M,P,Y,A,Q,H,R,[use_inf])
%
% In:
%   M - NxK matrix of K mean estimates from Kalman filter
%   P - NxNxK matrix of K state covariances from Kalman Filter
%   Y - Sequence of K measurement as DxK matrix
%   A - NxN state transition matrix.
%   Q - NxN process noise covariance matrix.
%   H - DxN Measurement matrix.
%   R - DxD Measurement noise covariance.
%   use_inf - If information filter should be used (default 1)
%
% Out:
%   M - Smoothed state mean sequence
%   P - Smoothed state covariance sequence
%   
% Description:
%   Two filter linear smoother algorithm. Calculate "smoothed"
%   sequence from given Kalman filter output sequence
%   by conditioning all steps to all measurements.
%
% Example:
%   m = m0;
%   P = P0;
%   MM = zeros(size(m,1),size(Y,2));
%   PP = zeros(size(m,1),size(m,1),size(Y,2));
%   for k=1:size(Y,2)
%     [m,P] = kf_predict(m,P,A,Q);
%     [m,P] = kf_update(m,P,Y(:,k),H,R);
%     MM(:,k) = m;
%     PP(:,:,k) = P;
%   end
%   [SM,SP] = tf_smooth(MM,PP,A,Q,H,R,Y);
%
% See also:
%   KF_PREDICT, KF_UPDATE

% History:
%   
%   02.8.2007 JH Changed the name to tf_smooth
%   26.3.2007 JH Fixed a bug in backward filter with observations having
%                having more than one dimension.
%             
% Copyright (C) 2006 Simo Särkkä
%               2007 Jouni Hartikainen
%
% $Id: tf_smooth.m 111 2007-09-04 12:09:23Z ssarkka $
%
% This software is distributed under the GNU General Public 
% Licence (version 2 or later); please refer to the file 
% Licence.txt, included with the software, for details.
%

function [M,P] = tf_smooth(M,P,Y,A,Q,H,R,use_inf)

  %
  % Check which arguments are there
  %
  if nargin < 4
    error('Too few arguments');
  end
  if nargin < 8
    use_inf = [];
  end
  
  if isempty(use_inf)
    use_inf = 1;
  end
  
  %
  % Run the backward filter
  %
  if use_inf
    zz = zeros(size(M));
    SS = zeros(size(P));
    IR = inv(R);
    IQ = inv(Q);
    z = zeros(size(M,1),1);
    S = zeros(size(M,1),size(M,1));
    for k=size(M,2):-1:1
      G = S / (S + IQ);
      S = A' * (eye(size(M,1)) - G) * S * A;
      z = A' * (eye(size(M,1)) - G) * z;
      zz(:,k)   = z;
      SS(:,:,k) = S;
      S = S + H'*IR*H;
      z = z + H'*IR*Y(:,k);
    end
  else
    BM = zeros(size(M));
    BP = zeros(size(P));
    IA = inv(A);
    IQ = IA*Q*IA';  
    fm = zeros(size(M,1),1);
    fP = 1e12*eye(size(M,1));
    BM(:,end) = fm;
    BP(:,:,end) = fP;
    for k=(size(M,2)-1):-1:1
      [fm,fP] = kf_update(fm,fP,Y(:,k+1),H,R);
      [fm,fP] = kf_predict(fm,fP,IA,IQ);
      BM(:,k) = fm;
      BP(:,:,k) = fP;
    end
  end

  %
  % Combine estimates
  %
  if use_inf
    for k=1:size(M,2)-1
      G = P(:,:,k) * SS(:,:,k) / (eye(size(M,1)) + P(:,:,k) * SS(:,:,k));
      P(:,:,k) = inv(inv(P(:,:,k)) + SS(:,:,k));
      M(:,k) = M(:,k) + P(:,:,k) * zz(:,k) - G * M(:,k);
    end
  else
    for k=1:size(M,2)-1
      tmp = inv(inv(P(:,:,k)) + inv(BP(:,:,k)));
      M(:,k) = tmp * (P(:,:,k)\M(:,k) + BP(:,:,k)\BM(:,k));
      P(:,:,k) = tmp;
    end
  end