graph.m

particle filter 粒子滤波器 matlab工具箱

function [simobj, cfilters]=graph(varargin);
% Graph method for the simulator class.
% Takes a simulator object and a filter, or a cell array of filters. Simulates data,
% filters it and visualizes the result.
%
% Syntax: (* = optional)
%
% graph(simobj, filtarray, steps, states*, u*, t*);
% graph(simobj, filtarray, steps, states*, u*, Ts*);
%
% In arguments:
%
% 1. simobj
%	simulator object that will be used for the filtering
% 2. filtarray
%	A filter object, or cell array of filter objects for use with the simulated data.
%	If a filter object is empty, the model assigned to the simulator object will be
%	assigned to the empty filter object.
% 3. steps
%	A positive integer containing the number of desired simulation steps.
%	These will be carried out, filtered and visualized. Note that no previous simulation or
%	filtered data (from the objects) will be used.
% 4* states
%	A scalar or a vector containing the states to be visualized.
% 4* []
%	'states' is set to 1, ie the first state is plotted.
% 5* u
%	A matrix u(t) containg deterministic data.
% 5* []
%	No u(t) will be used in the calculations.
% 6* Ts
%	A vector containing the desired sample points, where Ts(k) represents the
%	time of step k.
% 6* t
%	The time of the first simulation step. The discrete times of the succeeding steps will be
%	calculated using the T property of the assigned model.
%	The result is a generated sample point vector, Ts, that will be stored in the
%	simulator object.
% 6* []
%	't' will be set to simobj.t and the sample points, 'Ts', will be generated on
%	the basis of this.

% 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.

simobj=varargin{1};
if isempty(simobj)
	error('The simulator object has no model!');
end
cfilters=varargin{2};
steps=varargin{3};
T=get(get(simobj,'model'),'T');		% Get the discrete time of the model

if isa(cfilters,'cell')
	% A cell array was supplied. Calculate its length!
	filters=length(cfilters);
else
	% A single object was supplied. Make it into a cell array for compability!
	cfilters={cfilters};
	filters=1;
end

% Since these arguments defaults to [], we don't have to go through the long procedure with them
if nargin>=5; u=varargin{5}; else; u=[]; end;

% Declare the arguments
states=[];

% Fetch arguments, if they exist
if nargin>=4; states=varargin{4}; end

% If an empty argument, or no argument at all, was supplied - use its default value!
if isempty(states)
	states=1;
end

% Default t, Ts
t=simobj.t;			% Use t stored in the simulator object
Ts=(t:T:(t+(steps-1)*T))';	% Calculate sample points

% Fetch t or Ts, if they are supplied and non-empty
if nargin>=6;			% Ts or t was supplied by the user
	t=varargin{6};
	if length(t)>1
		if length(t)~=steps
			error('Incorrect size of supplied Ts vector');
		else
			% A sample point vector Ts was supplied. Use it!
			if size(t,2)>1	% Ts is a row vector (we expect only one-dimensional vectors)
				Ts=t';	% Transpose and copy it!
			else
				Ts=t;	% Ts is a column vector. Just copy it!
			end
		end
	elseif length(t)	% Don't overwrite old Ts if the new t=[]
		Ts=(t:T:(t+(steps-1)*T))'; % Generate new sample points with respect to t
	end
end

% Check if any filter object is empty. If so, use the model of the simulator object.
for j=1:filters
	if isempty(cfilters{j})
		filtobj=cfilters{j};	% set.m needs a named variable
		cfilters{j}=set(filtobj,'model',simobj.model);
	end;
end		

% Start the action
hf=figure('Name','Graph');
wbar = waitbar(0,'Simulating...');

legendcell=cell(filters+1,1);
legendcell{1}=class(simobj);

[y xtrue]=simulate(simobj,steps,u,Ts);
figure(hf);
plot(Ts,xtrue(states,:),getcolors(1));

%drawnow
hold on

for j=1:filters
	waitbar(j/(filters+1),wbar,strcat('Filtering: ',class(cfilters{j})));
	figure(wbar);
	drawnow
	% The assignin construction introduces some "redundant" code, since
	% the filter command isn't able to operate on the cell array directly.
	filtobj=cfilters{j};
	[xhat xpred]=filter(filtobj,y,u,Ts);
	% The filter object is written back into the array for simplicity.
	cfilters{j}=filtobj;
	figure(hf);
	plot(Ts,xhat(states,:),getcolors(j+1));
%	drawnow
	legendcell{j+1}=class(cfilters{j});
end;

waitbar(1,wbar,'Complete!');
drawnow

hold off
xlabel('Time (t)')
legend(legendcell{:})

close(wbar);