mupdate.m

particle filter 粒子滤波器 matlab工具箱

function [obj, xhat, xhat_p]=mupdate(obj,y,varargin)
% Measurement update method for the SIR particle filter
%
% Syntax: (* = optional)
%
% [sirobj, xhat, xhat_particles] = mupdate(sirobj, y, u*, xpred_particles*, t*);
%
% In arguments:
%
% 1. sirobj
%	SIR filter object that will be used for the measurement update.
% 2. y
%	The data, y(t), to be filtered. 
% 3* u
%	A scalar or column 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_particles will be set to ekfobj.xpred_particles.
% 5* t
%	The time of the actual filtering step. Ts will be set to this value
% 5* []
%	t will be set to ekfobj.t, and also Ts will be set to this value
%
% Out arguments:
%
% 1. sirobj
%	SIR object containg the result of the filter operation.
% 2. xhat
%	Estimate of x
%	This can also be accessed from the object using the sirobj.xhat property
% 3. xhat_particles
%	The particle swarm used to calculate xhat.
%	This can also be accessed from the object using the sirobj.xhat_particles 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; t=varargin{3}; 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(t)
	t=obj.t;
end

% Measurement update block
q=pyx(obj.model,y,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=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.y=y;
obj.u=ue;
obj.Ts=Ts;
obj.t=t;