animate.m

particle filter 粒子滤波器 matlab工具箱

function animate(varargin);
% Animation 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)
%
% animate(simobj, filtarray, steps, state*, axes*, u*, t*, delay*);
% animate(simobj, filtarray, steps, state*, axes*, u*, Ts*, delay*);
% animate(simobj, filtarray, steps, states*, axes*, u*, t*, delay*);
% animate(simobj, filtarray, steps, states*, axes*, u*, Ts*, delay*);
%
% 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* state
%	The state x(state) will be animated, in ONE-DIMENSION (using the plot1d method).
% 4* states
%	A pair of integers [sx, sy].
%	The x and y axes will represent x(sx) and x(sy) respectively.
%	This will result in a TWO-DIMENSIONAL plot (using the plot2d method).
% 4* []
%	If x contains multiple states, 'states' is set to [1 2] and
%	the plot will be two-dimensional. If not, 'state' is set to 1 and a
%	one-dimensional plot will be done.
% 5* axes
%	This argument is a rather complex one.
%
%	*** If 'axes' is a vector [x1 x2 y1 y2] of length 4: ***
%
%       'axes' = [x1 x2 y1 y2] sets the axes to these fixed values
%	after each plot (using Matlab's axis command).
%	Hence, we get FIXED AXES that don't change with time
%
%	*** If 'axes' is a vector [w h] of length 2: ***
%
%	In the TWO-DIMENSIONAL case, a vector with two elements [w h] centers the axes
%	around the simulated x value.
%	The width and the height of the graph is set to 2*w and 2*h respectively.
%	This results in MOVING AXES, whose location relative origo change with time.
%
%	In the ONE-DIMENSIONAL case, a vector with two elements [w h] sets the axes
%	to: x1=-w, x2=w, y1=0, y2=h and we get FIXED AXES.
%
% 5* []
%	The axes are set by the internal Matlab plot command.
% 6* u
%	A matrix u(t) containg deterministic data.
% 6* []
%	No u(t) will be used in the calculations.
% 7* Ts
%	A vector containing the desired sample points, where Ts(k) represents the
%	time of step k.
% 7* 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.
% 7* []
%	't' will be set to simobj.t and the sample points, 'Ts', will be generated on
%	the basis of this.
% 8* delay
% 	A double specifying the pause length in seconds.
%	If a negative value is given, the method will wait for a key to be pressed
%	after each frame.
% 8* []
%	'delay' is set to 0.25.

% 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; ax=varargin{5}; else; ax=[]; end;
if nargin>=6; u=varargin{6}; else; u=[]; end;

% Declare the arguments
states=[];
delay=[];

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

% If an empty argument, or no argument at all, was supplied - use its default value!
if isempty(states)
	% If no states argument was specified, use these default values.
	if size(get(simobj.model,'x0'),1)>1
		% The system has multiple states. As default, visualize state 1 and 2 in 2d mode
		states=[1 2];
	else
		% The system has just one state. As default, visualize it in 1d mode
		states=1;
	end;
end

if isempty(delay)
	delay=0.25;	% delay default
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>=7;			% Ts or t was supplied by the user
	t=varargin{7};
	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		

figure
% Animation loop
for i=1:steps
	uelement=extractelement(u,i);	% u(:,n), or [] if u is empty
	t=Ts(i);			% t for this step
	[y xtrue]=simulate(simobj,1,uelement,t);
	plot(simobj,states);
	for j=1:filters
		% The assignin construction introduces some "redundant" code, since
		% the filter command isn't able to operate on the cell array directly.
		filtobj=cfilters{j};
		filter(filtobj,y,uelement,t);
		cfilters{j}=filtobj;
		hold on
		plot(cfilters{j},states);
	end;

	% Set the axes if desired. ax=[] means no action
	if length(ax)==2
		% ax = [w h]
		if length(states)>1
			% Two-dimensional plot
			% MOVING AXES
			axis([xtrue(states(1))-ax(1) xtrue(states(1))+ax(1) xtrue(states(2))-ax(2) xtrue(states(2))+ax(2)])
		else
			% One-dimensional plot
			% FIXES AXES
			axis([-ax(1) ax(1) 0 ax(2)])
		end
	elseif length(ax)==4
		% ax = [x1 x2 y1 y2]
		% FIXED AXES
		axis(ax)
	end;
		
	drawnow
	hold off
	
	if delay<0
		pause;
	else
		pause(delay);
	end;
end;