%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