filter.m

particle filter 粒子滤波器 matlab工具箱

function [xhat,xpred]=filter(varargin);
% Filter method for the SIR particle filter
%
% Syntax: (* = optional)
%
% [xhat, xpred] = filter(sirobj, y, u*, Ts*);
% [xhat, xpred] = filter(sirobj, y, u*, t*);
% [xhat, xpred] = filter(sirobj, simobj, u*, Ts*);
% [xhat, xpred] = filter(sirobj, simobj, u*, t*);
%
% In arguments:
%
% 1. sirobj
%	SIR filter object that will be used for the filtering
% 2. y
%	The data, y(t), to be filtered. 
% 2. simobj
%	A simulator object, or any other object containing the necessary properties.
%	simobj.y, simobj.u and simobj.Ts will be extracted and used.
%	If u, Ts or t is given as non-empty arguments when the filter method is called, those
%	values will override the parameter values fetched from the simulator object.
% 3* u
%	A matrix u(t) containg the 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 filtering 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
%	filter object.
% 4* []
%	If a vector (y) was supplied as input data y(t), Ts will
%	be generated by using t=sirobj.t as the time of the first step.
%	If a simulator object was given instead, Ts is directly set to simobj.Ts.
%
% Out arguments:
%
% 1. xhat
%	Estimate of x
%	This can also be accessed from the object using the sirobj.xhat property
% 2. xpred
%	One-step prediction of x
%	This can also be accessed from the object using the sirobj.xpred property

% 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');

% Analyse the input data
varargin2=varargin{2};	%Faster than accessing the cell array multiple times
if isa(varargin2,'double')
	% A vector was given
	y=varargin2;
	steps=size(y,2);
	u=[];
	t=obj.t;			% Use t stored in the object
	Ts=t:T:(t+(steps-1)*T);		% Calculate sample points
else
	% A simulator object was given. Extract its data
	y=get(varargin2,'y');
	if isempty(y);
		error('Empty simulator object');
	end;
	u=get(varargin2,'u');	% Use the u vector stored in the simulation object
	Ts=get(varargin2,'Ts');		% Use the sample points of the simulation
	steps=size(y,2);
end

if nargin>=3;			% u was specified in the argument list
	varargin3=varargin{3};	% Optimization: Access the cell array as few times as possible
	if length(varargin3)	% Ignore if the new u=[] (to preserve the old settings)
		u=varargin3;	% Override current settings (possibly from the simulator object)
		if size(u,2)~=steps
			error('The lengths of u and y don''t match');
		end
	end
end

% The user doesn't usually specify Ts or t as arguments to the filter function. The objects
% are supposed to keep track of the current times and generate sample points, and the code
% is optimized for this. That's why it's possible that we'll calculate the sample points twice
% if t is supplied as an argument. This specific case will hopefully be rare...
%
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
xhat=zeros(states,steps);
xpred=zeros(states,steps);

xpred_p=obj.xpred_particles;

for k=1:steps
	t=Ts(k);
	ue=extractelement(u,k);			% u(:,k) or [] if u is undefined

	% Measurement update block
	q=pyx(model,y(:,k),xpred_p,t,ue);	% Calculate importance weights
	q=q/sum(q);				% Normalize weights
	xhat_p=feval(obj.resampler,xpred_p,q);	% Measurement update. SIR: Resample at every step
	xhat(:,k)=mean(xhat_p,2);		% Save filtered data

	% Time update block
	xpred_p=f_general(model,xhat_p,t,ue);	% Time update
	xpred(:,k)=mean(xpred_p,2);		% Save predicted data
end

t=t+T;		% Update time t

% Are the history buffers enabled?
if obj.historysize
	% Yes, renew them
	historysize=obj.historysize;	% Optimized for the case when history is disabled
	obj.xhat_history=append(obj.xhat_history,xhat,historysize);
	obj.xpred_history=append(obj.xpred_history,xpred,historysize);
	obj.y_history=append(obj.y_history,y,historysize);
	obj.u_history=append(obj.u_history,u,historysize);
	obj.Ts_history=append(obj.Ts_history,Ts,historysize);
end

% Store relevant variables in the object
obj.xhat=xhat;
obj.xpred=xpred;
obj.y=y;
obj.u=u;
obj.xpred_particles=xpred_p;
obj.xhat_particles=xhat_p;
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);