%ERTS_SMOOTH1 Extended Rauch-Tung-Striebel smoother
%
% Syntax:
% [M,P,D] = ERTS_SMOOTH1(M,P,A,Q,[a,W,param,same_p])
%
% In:
% M - NxK matrix of K mean estimates from Unscented Kalman filter
% P - NxNxK matrix of K state covariances from Unscented Kalman Filter
% 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 [M,P,D] = erts_smooth1(M,P,A,Q,a,W,param,same_p)
%
% Check which arguments are there
%
if nargin < 3
A = [];
end
if nargin < 4
Q = [];
end
if nargin < 5
a = [];
end
if nargin < 6
W = [];
end
if nargin < 7
param = [];
end
if nargin < 8
same_p = [];
end
%
% Apply defaults
%
if isempty(A)
A = eye(size(M,1));
end
if isempty(Q)
Q = zeros(size(M,1));
end
if isempty(W)
W = eye(size(M,1),size(Q,2));
end
if isempty(same_p)
same_p = 1;
end
%
% Extend Q if NxN matrix
%
if size(Q,3)==1
Q = repmat(Q,[1 1 size(M,2)]);
end
%
% Run the smoother
%
D = zeros(size(M,1),size(M,1),size(M,2));
for k=(size(M,2)-1):-1:1
if isempty(param)
params = [];
elseif same_p
params = param;
else
params = param{k};
end
%
% Perform prediction
%
if isempty(a)
m_pred = A*M(:,k);
elseif isnumeric(a)
m_pred = a;
elseif isstr(a) | strcmp(class(a),'function_handle')
m_pred = feval(a,M(:,k),params);
else
m_pred = a(M(:,k),params);
end
if isnumeric(A)
F = A;
elseif isstr(A) | strcmp(class(A),'function_handle')
F = feval(A,M(:,k),params);
else
F = A(M(:,k),params);
end
if isnumeric(W)
B = W;
elseif isstr(W) | strcmp(class(W),'function_handle')
B = feval(W,M(:,k),params);
else
B = W(M(:,k),params);
end
P_pred = F * P(:,:,k) * F' + B * Q(:,:,k) * B';
C = P(:,:,k) * F';
D(:,:,k) = C / P_pred;
M(:,k) = M(:,k) + D(:,:,k) * (M(:,k+1) - m_pred);
P(:,:,k) = P(:,:,k) + D(:,:,k) * (P(:,:,k+1) - P_pred) * D(:,:,k)';
end