sis.m

particle filter 粒子滤波器 matlab工具箱

function obj=sis(varargin)
% Constructor for the SIS particle filter
%
% Syntax: (* = optional)
%
% sisobj = sis(model, N*, Nth*, resampler*, t*);
% sisobj = sis(simobj, N*, Nth*, resampler*);
%
% In arguments:
%
% 1. model
%	The model object that will be used for the filtering.
% 1. simobj
%	If a simulator object is given as the only argument, the model assigned to this
%	object will be used as the filter model. Furthermore, if the simulator object contains
%	simulation data, the object will be sent to the SIS filter function.
%	The data will be filtered, and the result is returned embedded in the newly created
%	SIS object.
% 2* N
%	Number of particles used in the filter algorithm
% 2* []
%	N is set to 1000 particles
% 3* Nth
%	Resampling threshold
% 3* []
%	'Nth' is set to 'N'.
% 4* resampler
%	A handle to the resampling function.
% 4* []
%	resampler is set to @rs_simple (the simple resampling algorithm)
% 5* t
% 	Current time.
% 	Will be used as the time of the next filtering step, if nothing else is
% 	specified in the call to the filter method. See 'help sis/filter' for more
% 	information.
% 5* []
%	t=0 will be used
%
% Out arguments:
%
% 1. sisobj
%	SIS object, ready to be used for filtering.
%
% For more help, type 'props(sis)'

% 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 nargin==0
	% Empty constructor
	% This case is treated further down, so do nothing here...
	% (We need N to initialize x0, so it's nicer to do that below,
	% however, this block is not removed for consistency reasons)
elseif isa(varargin{1},'simulator')
	% A simulator object was supplied.
	simobj=varargin{1};
	model=get(simobj,'model');
	if isempty(model)
		error('The simulator object is empty! No model can be extracted.');
	end

	[N,Nth,resampler]=extractparams(varargin{:});

	obj=sis(model,N,Nth,resampler);		% Call the constructor again, using the model of the simulator
	if ~isempty(get(simobj,'y'))
		% The object contains data. Filter it!
		filter(obj,simobj);
		% To follow the Matlab OO standard, use the row below instead:
		% obj=filter(obj,simobj);
	end;
	return;			% Exit
elseif isa(varargin{1},'sis')
	% Copy constructor
	obj = varargin{1};	%Copy
	return;			%Exit
end

[N,Nth,resampler,t]=extractparams(varargin{:});

% Now we have N, so let's initialize the model and x0
if nargin>0
	model=varargin{1};
	x0_p=calc_x0(model,N);
else
	model=[];
	x0_p=[];
end;

% Write the properties
obj.model=model;
obj.t=t;
obj.Ts=[];
obj.xhat=[];
obj.xpred=[];
obj.u=[];
obj.y=[];
obj.N=N;
obj.Nth=Nth;
obj.resampler=resampler;
obj.xpred_particles=x0_p;
obj.xhat_particles=[];
obj.q=repmat(1/N,1,N);			% Initialize importance weights
obj.historysize=0;
obj.xhat_history=[];
obj.xpred_history=[];
obj.u_history=[];
obj.y_history=[];
obj.Ts_history=[];
obj.description='SIS Particle Filter (SIS)';
obj=class(obj,'sis');