f_general.m

particle filter 粒子滤波器 matlab工具箱

function v=f_general(obj,x,t,u,varargin)
% Calculates: f(x,t) + gu(x,t)*u(t) + gw(x,t)*w(t)
%
% Syntax: (* = optional)
%
% x_new = f_general(model, x, t, u, w*);
%
% In arguments:
%
% 1. model
%	Model object
% 2. x
%	Column vector or scalar containing x(t)
% 3. t
%	Scalar containing the time of the operation.
% 4. u
%	Column vector or scalar containing deterministic data for this particular step.
% 5* w
%	Column vector or scalar containing w(t) for this particular step.
% 5* []
%	w(t) is drawn from model.w
%
% Out arguments:
%
% 1. x_new
%	The result of the operation, x(t+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.

% Evaluate f(x,t)
v=eval(obj.f,x,t,[],[]);

% We have to consider the following facts:
% Negations are expensive
% Two separate if statements are faster than one statement containing logical and/or's.

% Ugly, but optimized for speed!

% length(expr) == ~isempty(expr)

% Handle the process noise
w=[];		% Default: draw noise from object
if nargin>=5 
	w=varargin{1};		% w was supplied as an argument
end;
if isempty(w)
	% No w was given as an argument. Use model.w to draw random data
	% if size(v,2)>1, we are dealing with a particle swarm and must draw a matrix
	w=random(obj.w, t, 1, size(v,2));
	v=v+eval(obj.gw,x,t,[],[])*w;		% g(x,t)*w
elseif w~=0	% use it if it's non-zero!
		v=v+eval(obj.gw,x,t,[],[])*w;	% g(x,t)*w
end	% if w is zero, don't do anything. This means no process noise will be added!

% Add u, if it was given and non-empty
if length(u)
	gu=obj.gu;
	if ~isempty(gu);
		% ...but only if gu is defined.
		v=v+eval(gu,x,t,[],[])*u;
	end
end