%EKF_SMOOTH1  Extended Rauch-Tung-Striebel smoother
%
% Syntax:
%   [M,P,D] = ekf_smooth1(M,P,A,Q,[a,W,param,same_p])
%
% In:
%   M - NxK matrix of K mean estimates from Unscented Kalman filter
%   Pplus - NxNxK matrix of K state covariances from Unscented Kalman Filter
%   Pminus -
%   A - Derivative of a() with respect to state as
%       matrix, inline function, function handle or
%       name of function in form A(x,param)                 (optional, default eye())
%   Q - Process noise of discrete model                       (optional, default zero)
%   a - Mean prediction E[a(x[k-1],q=0)] as vector,
%       inline function, function handle or name
%       of function in form a(x,param)                        (optional, default A(x)*X)
%   W - Derivative of a() with respect to noise q
%       as matrix, inline function, function handle
%       or name of function in form W(x,param)                (optional, default identity)
%   param - Parameters of a. Parameters should be a single cell array, vector or a matrix
%           containing the same parameters for each step or if different parameters
%           are used on each step they must be a cell array of the format
%           { param_1, param_2, ...}, where param_x contains the parameters for
%           step x as a cell array, a vector or a matrix.     (optional, default empty)
%   same_p - 1 if the same parameters should be
%            used on every time step                          (optional, default 1)
%                                   
%                         
%
% Out:
%   K - Smoothed state mean sequence
%   P - Smoothed state covariance sequence
%   D - Smoother gain sequence
%   
% Description:
%   Extended Rauch-Tung-Striebel smoother algorithm. Calculate
%   "smoothed" sequence from given Kalman filter output sequence by
%   conditioning all steps to all measurements.
%
% Example:
%
% See also:
%   EKF_PREDICT1, EKF_UPDATE1

% History:
%   04.05.2007 JH Added the possibility to pass different parameters for a and h
%                 for each step.
%   2006       SS Initial version.  
%
% Copyright (C) 2006 Simo Särkkä
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: erts_smooth1.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 [Meks,Peks] = ekf_smooth1(M,P,Mminus,Pminus,A,params)


nsamples = size(P,3);

Peks = zeros(size(P));
Meks = zeros(size(M));
Peks(:,:,nsamples) = P(:,:,nsamples);

Meks(:,nsamples) = M(:,nsamples);


for k=(nsamples-1):-1:1
    
    %
    % Perform prediction
    
    if isstr(A) | strcmp(class(A),'function_handle')
        F = feval(A,M(:,k),params);
    end
    
    S = P(:,:,k) * F' * inv(Pminus(:,:,k+1));
    Meks(:,k) = M(:,k) + S * (Meks(:,k+1) - Mminus(:,k+1));
    Peks(:,:,k) = P(:,:,k) - S * (Pminus(:,:,k+1) - Peks(:,:,k+1)) * S';
end