%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