mupdate.m

particle filter 粒子滤波器 matlab工具箱

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

% Declare the arguments
xpred=[];
Ppred=[];
t=[];

% Fetch arguments, if they exist
if nargin>=4; xpred=varargin{2}; end;
if nargin>=5; Ppred=varargin{3}; end;
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)
	xpred=xpred=obj.xpred(:,end);	% xpred default
end

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

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

% Measurement update block
model=obj.model;

R=cov_e(model,t);
H=hgradx_general(model,xpred, t, ue);
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,[]));	
P=Ppred-K*H*Ppred;

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