simulate.m

particle filter 粒子滤波器 matlab工具箱

function [y_array,xtrue_array]=simulate(varargin);
% Simulation method for the simulator object
%
% Syntax: (* = optional)
%
% [y, x] = simulate(simobj, steps, u*, Ts*);
% [y, x] = simulate(simobj, steps, u*, t*);
%
% In arguments:
%
% 1. simobj
%	Simulator object that will be used for the simulation
% 2. steps
%	Number of simulated steps 
% 3* u
%	A matrix u(t) containg deterministic data.
% 3* []
%	No u(t) will be used in the calculations.
% 4* Ts
%	A vector containing the desired sample points, where Ts(k) represents the
%	time of step k.
% 4* 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.
% 4* []
%	't' will be set to simobj.t and the sample points, 'Ts', will be generated on
%	the basis of this.
%
% Out arguments:
%
% 1. y
%	Data y(t) that was generated by the simulation.
%	Often used as input data for filter algorithms.
% 2. x
%	The corresponding ("true") x(t).
%	Often used for evaulating filters, by comparing the true values to the filter estimates.
%
% For more help, type 'props(simulator)'

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

% This is no good solution, and should be removed in the next version.
% Instead the object should be returned as the first output argument.
if isempty(inputname(1))
	error('The first argument must be a named variable.')
end

obj=varargin{1};
model=obj.model;
states=length(get(model,'x0'));
T=get(model,'T');

steps=varargin{2};

if nargin>=3;			% u was specified in the argument list
	u=varargin{3};	% Override current settings
	if length(u);
		if size(u,2)~=steps
			error('The lengths of u and y don''t match');
		end
	end
else
	u=[];
end

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

% Fetch t or Ts, if they are supplied and non-empty
if nargin>=4;			% Ts or t was supplied by the user
	t=varargin{4};
	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,1)>1	% Ts is a column vector (we expect only one-dimensional vectors)
				Ts=t';	% Transpose and copy it!
			else
				Ts=t;	% Ts is a row 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

% For faster handling, dimension the arrays before anything is written to them
xtrue_array=zeros(states,steps);
%y_array=zeros(?,steps);		% We can do an extra h_general call to find out
					% the dimensions. Is it faster? Investigate...

xtrue=obj.x0;					% x0
for k=1:steps
	ue=extractelement(u,k);			% u(:,k) or [] if u is undefined
	t=Ts(k);
	xtrue_array(:,k)=xtrue;
	y_array(:,k)=h_general(model, xtrue, t, ue);
	xtrue=f_general(model, xtrue, t, ue);		% Time update
end

t=t+get(model,'T');	% Update time

% Are the history buffers enabled?
if obj.historysize
	% Yes, renew them
	historysize=obj.historysize;
	obj.x_history=append(obj.x_history,xtrue_array,historysize);
	obj.y_history=append(obj.y_history,y_array,historysize);
	obj.u_history=append(obj.u_history,u,historysize);
	obj.Ts_history=append(obj.Ts_history,Ts,historysize);
end

obj.x=xtrue_array;
obj.y=y_array;
obj.u=u;
obj.x0=xtrue;
obj.Ts=Ts;
obj.t=t;

% See the comments above.
% Overwrite the source object with the modified object.
% I think this should be removed to follow Matlab standards,
% i.e. returning the object as the first output argument.
assignin('caller',inputname(1),obj);