tupdate.m

particle filter 粒子滤波器 matlab工具箱

function [obj, xpred, xpred_p]=tupdate(obj,varargin)
% Time update method for the SIS particle filter
%
% Syntax: (* = optional)
%
% [sisobj, xpred, xpred_particles] = tupdate(sisobj, u*, xhat_particles*, t*);
%
% In arguments:
%
% 1. sisobj
%	SIS filter object that will be used for the measurement update.
% 2* u
%	A scalar or row vector u(t) containing the deterministic data for the particular step.
% 2* []
%	No u(t) will be used in the calculations.
% 3* xhat_particles
%	A matrix representing the last particle swarm used for estimation of x
% 3* []
%	xhat will be set to ekfobj.xhat_particles.
% 4* t
%	The time of the actual filtering step. Ts will be set to this value
% 4* []
%	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. xpred
%	One-step prediction of x
%	This can also be accessed from the object using the sisobj.xpred property
% 2. xpred_particles
%	The particle swarm used to calculate xpred.
%	This can also be accessed from the object using the sisobj.xpred_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>=2; ue=varargin{1}; else; ue=[]; end;

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

% Fetch arguments, if they exist
if nargin>=3; xhat_p=varargin{2}; end;
if nargin>=4; t=varargin{3}; end;

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

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

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

Ts=t;			% One samplepoint used.
t=t+get(model,'T');	% Update time

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