ekf.m

MATLAB非线性滤波工具箱 在“中国国防科技论坛 ”找到的

function obj=ekf(varargin)
% Constructor for the EKF (Extended Kalman Filter) filter
%
% Syntax: (* = optional)
%
% ekfobj = ekf(model, t*);
% ekfobj = ekf(simobj);
%
% In arguments:
%
% 1. model
%	The model object that will be used for the filtering.
% 1. simobj
%	If a simulator object is given as the only argument, the model assigned to this
%	object will be used as the filter model. Furthermore, if the simulator object contains
%	simulation data, the object will be sent to the EKF filter function.
%	The data will be filtered, and the result is returned embedded in the newly created
%	EKF object.
% 2* t
% 	Current time.
% 	Will be used as the time of the next filtering step, if nothing else is
% 	specified in the call to the filter method. See 'help ekf/filter' for more
% 	information.
% 2* []
%	t=0 will be used
%
% Out arguments:
%
% 1. ekfobj
%	EKF object, ready to be used for filtering.
%
% 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.

if nargin==0
	% Empty constructor
	x=[];
	Ppred=[];	
	model=[];
elseif isa(varargin{1},'ekf')
	% Copy constructor
	obj = varargin{1};	%Copy
	return;			%Exit
elseif isa(varargin{1},'simulator')
	% A simulator object was supplied.
	simobj=varargin{1};
	model=get(simobj,'model');
	if isempty(model)
		error('The simulator object is empty! No model can be extracted.');
	end
	obj=ekf(model);	% Call the constructor again, using the model of the simulator
		if ~isempty(get(simobj,'y'))
		% The object contains data. Filter it!
		filter(obj,simobj);
		% To follow the Matlab OO standard, use the row below instead:
		% obj=filter(obj,simobj);
	end;
	return;			% Exit
else
	% Standard constructor
	model=varargin{1};
	x=calc_x0(model,1);		% Calculate x0
	Ppred=get(model,'P0');		% Copy P0 from the model
	model=initgradients(model);	% Initialize gradients
end

% Declare the arguments
t=[];

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

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

% Initialize property structure
obj.model=model;
obj.t=t;
obj.Ts=[];
obj.xhat=[];
obj.xpred=x;
obj.u=[];
obj.y=[];
obj.Phat=[];
obj.Ppred=Ppred;
obj.historysize=0;
obj.xhat_history=[];
obj.xpred_history=[];
obj.y_history=[];
obj.u_history=[];
obj.Phat_history={};
obj.Ppred_history={};
obj.Ts_history=[];
obj.description='Extended Kalman Filter (EKF)';
obj=class(obj,'ekf');