gradw.m

particle filter 粒子滤波器 matlab工具箱

function v=gradw(obj,x,t,w,u)
% Evaluates the gradient object at time t, with respect to w.
%
% Syntax: (* = optional)
%
% v = gradw(obj, x, t, w, u);
%
% In arguments:
%
% 1. obj
%	The xtable object containing the gradient.
%	Must be generated by a call to the 'initgradw' method before use.
% 2. x
%	A column vector or a scalar containing x(t).
% 3. t
%	A scalar containing the time t.
% 4. u
%	A column vector or scalar containing u(t).
% 5. w
%	A column vector or scalar containing w(t)
%
% Out arguments:
%
% 1. v
%	A matrix containing the gradient at time t.

% Toolbox for nonlinear filtering.
% Copyright (C) 2005  Jakob Ros閚 <[e-mail]>
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

if obj.interpolate
	tvec=find(obj.Ts>=t);

	if isempty(tvec)
		% t>Ts(max)
		Ts=obj.Ts;
		gradw=obj.gradw;
		idxup=length(Ts);
		idxlow=idxup-1;
		tup=Ts(idxup);
		tlow=Ts(idxlow); % An error here means that Ts contains too few elements for interpolation
		vlow=gradw(:,:,idxlow);
		vup=gradw(:,:,idxup);
		v=vup+(vup-vlow)./(tup-tlow).*(t-tup);			
		return;		
	end

	idxup=tvec(1);
	Ts=obj.Ts;
	tup=Ts(idxup);
	if tup==t
		% Right on target. No interpolation needed.
		v=obj.gradw(:,:,idxup);
	elseif idxup>1
		% t is somewhere in Ts
		gradw=obj.gradw;
		idxlow=idxup-1;
		tlow=Ts(idxlow);
		vlow=gradw(:,:,idxlow);
		vup=gradw(:,:,idxup);
		v=vlow+(vup-vlow)./(tup-tlow).*(t-tlow);
	else
		% idxup==1 and t~=tup, so t<Ts(min)
		gradw=obj.gradw;
		v=gradw(:,:,1)+(gradw(:,:,2)-gradw(:,:,1))./(Ts(2)-tup).*(t-tup);			
	end
else
	tvec=find(obj.Ts>=t);
	if isempty(tvec)
		v=obj.gradw(:,:,end);
	else
		v=obj.gradw(:,:,tvec(1));
	end
end