%ETF_SMOOTH1 Smoother based on two extended Kalman filters
%
% Syntax:
% [M,P] = ETF_SMOOTH1(M,P,Y,A,Q,ia,W,aparam,H,R,h,V,hparam,same_p_a,same_p_h)
%
% In:
% M - NxK matrix of K mean estimates from Kalman filter
% P - NxNxK matrix of K state covariances from Kalman Filter
% Y - Measurement vector
% 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)
% ia - Inverse prediction function as vector,
% inline function, function handle or name
% of function in form ia(x,param) (optional, default inv(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)
% aparam - 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)
% H - Derivative of h() with respect to state as matrix,
% inline function, function handle or name
% of function in form H(x,param)
% R - Measurement noise covariance.
% h - Mean prediction (measurement model) as vector,
% inline function, function handle or name
% of function in form h(x,param). (optional, default H(x)*X)
% V - Derivative of h() with respect to noise as matrix,
% inline function, function handle or name
% of function in form V(x,param). (optional, default identity)
% hparam - Parameters of h. See the description of aparam for the format of
% parameters. (optional, default aparam)
% same_p_a - If 1 uses the same parameters
% on every time step for a (optional, default 1)
% same_p_h - If 1 uses the same parameters
% on every time step for h (optional, default 1)
%
% Out:
% M - Smoothed state mean sequence
% P - Smoothed state covariance sequence
%
% Description:
% Two filter nonlinear smoother algorithm. Calculate "smoothed"
% sequence from given extended Kalman filter output sequence
% by conditioning all steps to all measurements.
%
% Example:
% [...]
%
% See also:
% ERTS_SMOOTH1, EKF_PREDICT1, EKF_UPDATE1, EKF_PREDICT2, EKF_UPDATE2
% History:
% 02.08.2007 JH Changed the name to etf_smooth1
% 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: etf_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 [M,P] = etf_smooth1(M,P,Y,A,Q,ia,W,aparam,H,R,h,V,hparam,same_p_a,same_p_h)
%
% Check which arguments are there
%
if nargin < 3
error('Too few arguments');
end
if nargin < 4
A = [];
end
if nargin < 5
Q = [];
end
if nargin < 6
ia = [];
end
if nargin < 7
W = [];
end
if nargin < 8
aparam = [];
end
if nargin < 9
H = [];
end
if nargin < 10
R = [];
end
if nargin < 11
h = [];
end
if nargin < 12
V = [];
end
if nargin < 13
hparam = [];
end
if nargin < 14
same_p_a = 1;
end
if nargin < 15
same_p_h = 1;
end
%
% Run the backward filter
%
BM = zeros(size(M));
BP = zeros(size(P));
%fm = zeros(size(M,1),1);
%fP = 1e12*eye(size(M,1));
fm = M(:,end);
fP = P(:,:,end);
BM(:,end) = fm;
BP(:,:,end) = fP;
for k=(size(M,2)-1):-1:1
if isempty(hparam)
hparams = [];
elseif same_p_h
hparams = hparam;
else
hparams = hparam{k};
end
if isempty(aparam)
aparams = [];
elseif same_p_a
aparams = aparam;
else
aparams = aparam{k};
end
[fm,fP] = ekf_update1(fm,fP,Y(:,k+1),H,R,h,V,hparams);
%
% Backward prediction
%
if isempty(A)
IA = [];
elseif isnumeric(A)
IA = inv(A);
elseif isstr(A) | strcmp(class(A),'function_handle')
IA = inv(feval(A,fm,aparams));
else
IA = inv(A(fm,aparams));
end
if isempty(W)
if ~isempty(Q)
B = eye(size(M,1),size(Q,1));
else
B = eye(size(M,1));
end
elseif isnumeric(W)
B = W;
elseif isstr(W) | strcmp(class(W),'function_handle')
B = feval(W,fm,aparams);
else
B = W(fm,aparams);
end
IQ = IA*B*Q*B'*IA';
[fm,fP] = ekf_predict1(fm,fP,IA,IQ,ia,[],aparams);
BM(:,k) = fm;
BP(:,:,k) = fP;
end
%
% Combine estimates
%
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