mupdate.m

particle filter 粒子滤波器 matlab工具箱

function [obj, xhat, xhat_p, q]=mupdate(obj,y,varargin)
% Measurement update method for the SIS particle filter
%
% Syntax: (* = optional)
%
% [sisobj, xhat, xhat_particles, q] = mupdate(sisobj, y, u*, xpred_particles*, t*);
%
% In arguments:
%
% 1. sisobj
%	SIS filter object that will be used for the measurement update.
% 2. y
%	The data, y(t), to be filtered. 
% 3* u
%	A scalar or row vector u(t) containing the deterministic data for the particular step.
% 3* []
%	No u(t) will be used in the calculations.
% 4* xpred_particles
%	A matrix representing the last predicted particle swarm of x
% 4* []
%	xpred will be set to ekfobj.xpred_particles.
% 5* q
%	A vector containing the previous importance weights.
% 5* []
%	q will be set to obj.q
% 6* t
%	The time of the actual filtering step. Ts will be set to this value
% 6* []
%	t will be set to ekfobj.t, and also Ts will be set to this value
%
% Out arguments:
%
% 1. sisobj
%	SIS object containg the result of the filter operation.
% 2. xhat
%	Estimate of x
%	This can also be accessed from the object using the sisobj.xhat property
% 3. xhat_particles
%	The particle swarm used to calculate xhat.
%	This can also be accessed from the object using the sisobj.xhat_particles property
% 4. q
%	The updated importance weights.
%	This can also be accessed from the object using the sisobj.q 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.

% Since ue defaults to [], we don't have to go through the long procedure with it
if nargin>=3; ue=varargin{1}; else; ue=[]; end;

% Declare the arguments
xpred_p=[];
t=[];

% Fetch arguments, if they exist
if nargin>=4; xpred_p=varargin{2}; end
if nargin>=5; q=varargin{3};
if nargin>=6; t=varargin{4}; end

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

if isempty(q)
	q=obj.q;
end

if isempty(t)
	t=obj.t;
end

N=obj.N;

% Measurement update block
q=pyx(obj.model,y,xpred_p,t,ue).*q;	% Calculate importance weights
q=q/sum(q);				% Normalize weights
if 1/sum(q.^2)<obj.Nth
	% Neff < Nth
	xhat_p=feval(obj.resampler,xpred_p,q);	% Measurement update.
	q=repmat(1/N,1,N);			% Initialize importance weights
else
	xhat_p=xpred_p;
end
xhat=mean(xhat_p,2);			% Save filtered data

Ts=t;	% One samplepoint used.

% Are the history buffers enabled?
if obj.historysize
	% Not supported yet.
	% A good solution is to write to the history at the time update. Investigate it...
	warning('This operation does not support history buffers');
end

% Store relevant variables in the object
obj.xhat=xhat;
obj.xhat_particles=xhat_p;
obj.xpred_particles=xpred_p;
obj.q=q;
obj.y=y;
obj.u=ue;
obj.Ts=Ts;
obj.t=t;