%URTS_SMOOTH2 Augmented form Unscented Rauch-Tung-Striebel smoother
%
% Syntax:
% [M,P,S] = URTS_SMOOTH2(M,P,f,Q,[f_param,alpha,beta,kappa,mat,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
% f - Dynamic model function as inline function,
% function handle or name of function in
% form a([x;w],param)
% Q - Non-singular covariance of process noise w
% f_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)
% alpha - Transformation parameter (optional)
% beta - Transformation parameter (optional)
% kappa - Transformation parameter (optional)
% mat - If 1 uses matrix form (optional, default 0)
% same_p - If 1 uses the same parameters
% on every time step (optional, default 1)
%
% Out:
% K - Smoothed state mean sequence
% P - Smoothed state covariance sequence
% D - Smoother gain sequence
%
% Description:
% Unscented Rauch-Tung-Striebel smoother algorithm. Calculate
% "smoothed" sequence from given Kalman filter output sequence by
% conditioning all steps to all measurements.
%
% Example:
% [...]
%
% See also:
% URTS_SMOOTH1, UKF_PREDICT2, UKF_UPDATE2
% Copyright (C) 2006 Simo S�rkk�
%
% $Id: urts_smooth2.m 481 2010-10-18 07:46:45Z jmjharti $
%
% 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] = urts_smooth2(M,P,f,Q,f_param,alpha,beta,kappa,mat,same_p)
%
% Check which arguments are there
%
if nargin < 4
error('Too few arguments');
end
if nargin < 5
f_param = [];
end
if nargin < 6
alpha = [];
end
if nargin < 7
beta = [];
end
if nargin < 8
kappa = [];
end
if nargin < 9
mat = [];
end
if nargin < 10
same_p = 1;
end
%
% Apply defaults
%
if isempty(mat)
mat = 0;
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(f_param)
params = [];
elseif same_p
params = f_param;
else
params = f_param{k};
end
MA = [M(:,k);zeros(size(Q,1),1)];
PA = zeros(size(P,1)+size(Q,1));
PA(1:size(P,1),1:size(P,1)) = P(:,:,k);
PA(1+size(P,1):end,1+size(P,1):end) = Q;
tr_param = {alpha beta kappa mat};
[m_pred,P_pred,C] = ...
ut_transform(MA,PA,f,params,tr_param);
C = C(1:size(M,1),:);
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