tupdate.m

particle filter 粒子滤波器 matlab工具箱

[obj, xpred, Ppred]=tupdate(obj, varargin)
% Time update method for the EKF (Extended Kalman Filter)
%
% Syntax: (* = optional)
%
% [ekfobj, xpred, Ppred] = mupdate(ekfobj, u*, xhat*, Phat*, t*);
%
% In arguments:
%
% 1. ekfobj
%	EKF filter object that will be used for the filtering
% 2* u
%	A matrix u(t) containg the deterministic data, for this particular step.
% 2* []
%	No u(t) will be used in the calculations.
% 3* xhat
%	Estimate of x
% 3* []
%	xhat will be set to ekfobj.xhat(:,end), ie the estimate of the last filter step.
% 4* Phat
%	Estimate of P
% 4* []
%	Phat will be set to ekfobj.Phat(:,:,end), ie the estimate of the last filter step.
% 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. ekfobj
%	EKF object containg the result of the time update operation.
% 2. xpred
%	One-step prediction of x
%	This can also be accessed from the object using the ekfobj.xpred property
% 3. 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.

% 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
x=[];
P=[];
t=[];

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

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

if isempty(P)
	P=obj.Phat(:,:,end);		% Phat default
end;

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

% Time update block
model=obj.model;

Q=cov_w(model,t);
F=fgradx_general(model, x, t, ue, 0);
G=fgradw_general(model, x, t, ue, []);
xpred=f_general(model, x, t, ue, 0);
Ppred=F*P*F'+G*Q*G';

Ts=t;
t=t+get(model,'T');	% Update time

% 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=x;
obj.xpred=xpred;
obj.y=y;
obj.u=ue;
obj.Phat=P;
obj.Ppred=Ppred;
obj.Ts=Ts;
obj.t=t;