filter.m

particle filter 粒子滤波器 matlab工具箱

function [xhat_array,xpred_array,Phat_array,Ppred_array]=filter(varargin);
% Filter method for the EKF (Extended Kalman Filter) filter
%
% Syntax: (* = optional)
%
% [xhat, xpred, Phat, Ppred] = filter(ekfobj, y, u*, Ts*);
% [xhat, xpred, Phat, Ppred] = filter(ekfobj, y, u*, t*);
% [xhat, xpred, Phat, Ppred] = filter(ekfobj, simobj, u*, Ts*);
% [xhat, xpred, Phat, Ppred] = filter(ekfobj, simobj, u*, t*);
%
% In arguments:
%
% 1. ekfobj
%	EKF 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=ekfobj.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 ekfobj.xhat property
% 2. xpred
%	One-step prediction of x
%	This can also be accessed from the object using the ekfobj.xpred property
% 3. Phat
%	Estimate of P
%	This can also be accessed from the object using the ekfobj.Phat property
% 4. Ppred
%	One-step prediction of P
%	This can also be accessed from the object using the ekfobj.Ppred property
%
% For more help, type 'props(ekf)'

% 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
Ppred_array=zeros(states,states,steps);
Phat_array=zeros(states,states,steps);
xhat_array=zeros(states,steps);
xpred_array=zeros(states,steps);

xpred=obj.xpred(:,end);  			% x_1|0
Ppred=obj.Ppred(:,:,end);			% P_1|0

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

	% Measurement update
	Q=cov_w(model,t);
	R=cov_e(model,t);
	H=hgradx_general(model,xpred, t, ue, 0);
	V=hgrade_general(model,xpred, t, ue, []);	% only relevant for the DGNL model
	K=Ppred*H'*inv(H*Ppred*H'+V*R*V');
	x=xpred+K*(y(:,k)-h_general(model,xpred,t,ue,0));	
	P=Ppred-K*H*Ppred;
	xhat_array(:,k)=x;
	Phat_array(:,:,k)=P;

	% Time update
	F=fgradx_general(model, x, t, ue, 0);
	G=fgradw_general(model, x, t, ue, []);		% Will draw w from noise object 
							% since no w argument is given
	xpred=f_general(model, x, t, ue, 0);
	Ppred=F*P*F'+G*Q*G';
	xpred_array(:,k)=xpred;
	Ppred_array(:,:,k)=Ppred;
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_array,historysize);
	obj.xpred_history=append(obj.xpred_history,xpred_array,historysize);
	obj.y_history=append(obj.y_history,y,historysize);
	obj.u_history=append(obj.u_history,u,historysize);
	obj.Phat_history=append3d(obj.Phat_history,Phat_array,historysize);
	obj.Ppred_history=append3d(obj.Ppred_history,Ppred_array,historysize);
	obj.Ts_history=append(obj.Ts_history,Ts,historysize);
end

% Store relevant variables in the object
obj.xhat=xhat_array;
obj.xpred=xpred_array;
obj.y=y;
obj.u=u;
obj.Phat=Phat_array;
obj.Ppred=Ppred_array;
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);