dgnl.m

particle filter 粒子滤波器 matlab工具箱

function obj = pfsys(varargin)
% Constructor for the DGNL (Discrete General Non-Linear) model
%
% x(t+T) = f(x,t,u,w)
%   y(t) = h(x,t,u,e)
%
% Syntax: (* = optional)
%
% model = dgnl(f, h, h_inv, x0, w, e, p0*, T*, xvars*, uvars*, wvars*, evars*);
%
% In arguments:
%
% 1. f
%	Data object containing f.
%	If a double (matrix), or a cell array of strings (or inlines) is given, it will be
%	used to create an apropriate data object.
% 2. h
%	Data object containing h
%	If a double (matrix), or a cell array of strings (or inlines) is given, it will be
%	used to create an apropriate data object.
% 3. h_inv
%	A function handle pointing to the inverse of h with respect to e(t).
%	The syntax of this function must be:
%	e = h_inv(y, x, t, u)
% 4. x0
%	A column vector containing the expectation of the initial state estimate.
%	Each row represents a state.
% 5. w
%	Noise object containing w.
%	It can also be a covariance matrix or a vector containing the diagonal of a
%	diagonal covariance matrix. This will be used to create a gaussian noise object
%	for use as w.
% 6. e
%	Noise object containing e
%	It can also be a covariance matrix or a vector containing the diagonal of a
%	diagonal covariance matrix. This will be used to create a gaussian noise object
%	for use as e.
% 7* p0
%	Noise object containing p0, that has to generate tuples of the same size as x0.
%	It can also be a covariance matrix or a vector containing the diagonal of a
%	diagonal covariance matrix. This will be used to create a gaussian noise object
%	for use as p0.
% 7* []
%	'p0' is set to independent gaussian noise with variance 1.
% 8* T
%	Sample time.
% 8* []
%	'T' is set to 1.
% 9* xvars
%	A cell array containing the names of the states.
% 9* []
%	'xvars' is set to {'x1', 'x2', 'x3', ...}
% 10* uvars
%	A cell array containing the names of the u(t) components.
% 10* []
%	'uvars' is set to {'u1', 'u2', '3', ...}
% 11* wvars
%	A cell array containing the names of the process noise components.
% 11* []
%	'wvars' is set to {'w1', 'w2', 'w3', ...}
% 12* evars
%	A cell array containing the names of the measurement noise components.
% 12* []
%	'evars' is set to {'e1', 'e2', 'e3', ...}
%
% Out arguments:
%
% 1. model
%	The resulting dgnl model object.

% 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
	% Will create a non-sense object, to satisfy Matlab standards
	obj.f=[];
	obj.h=[];
	obj.h_inv=[];
	obj.x0=[];
	obj.w=[];
	obj.e=[];
	obj.p0=[];
	obj.T=1;
	obj.xvars={};
	obj.uvars={};
	obj.wvars={};
	obj.evars={};
	obj.description='Discrete General Non-Linear model (DGNL)';
	obj=class(obj,'dgnl');
	return;				%Exit
elseif nargin==1
	argin=varargin{1};
	if isa(argin,'dnl')
		% Copy constructor
		obj = argin;		%Copy
		return;			%Exit
	end
end;

error(nargchk(6, 12, nargin));

% Fetch x0
x0=varargin{4};
if (size(x0,1)==1)&&(size(x0,2)>1);
	x0=x0';		% A column vector was supplied. Transpose it!
end;

% Declare the arguments
p0temp=[];
T=[];
xvars={};
uvars={};
wvars={};
evars={};

% Fetch arguments, if they exist
if nargin>=7; p0temp=varargin{7}; end;
if nargin>=8; T=varargin{8}; end;
if nargin>=9; xvars=varargin{9};  end;
if nargin>=10; uvars=varargin{10};  end;
if nargin>=11; wvars=varargin{11};  end;
if nargin>=12; evars=varargin{12};  end;

% If an empty argument, or no argument at all, was supplied - use its default value!
if isempty(p0temp)
	p0temp=eye(length(x0));	% p0 default, ind. gaussian noise with variance 1
end;

if isempty(T)
	T=1;			% T default
end;

if isempty(xvars)
	% Generate a xvar vector
	xvars=generatevars('x',length(x0));
elseif length(xvars)~=length(x0)
	error('The number of elements in ''xvars'' does not match the number of states');
end


% Initialize the noise (supplied as an argument or generated above)
p0=initnoise(p0temp);
w=initnoise(varargin{5});
e=initnoise(varargin{6});

if isempty(wvars)
	% Generate a wvar vector
	wvars=generatevars('w',get(w,'n'));
elseif length(wvars)~=get(w,'n')
	error('The number of elements in ''wvars'' does not match the number of process noise elements');
end

if isempty(evars)
	% Generate a wvar vector
	evars=generatevars('e',get(e,'n'));
elseif length(evars)~=get(e,'n')
	error('The number of elements in ''evars'' does not match the number of measurement noise elements');
end


% Initialize data.
f=initdata(varargin{1},xvars,uvars,wvars,1);
h=initdata(varargin{2},xvars,uvars,evars,1);
set(h,'wchar','e');
h_inv=varargin{3};

if isempty(uvars)
	% Generate a uvar vector
	uvars=joinvars(get(f,'uvars'),get(h,'uvars'));
end

%Initialize object data
obj.f=f;
obj.h=h;
obj.h_inv=h_inv;
obj.x0=x0;
obj.w=w;
obj.e=e;
obj.p0=p0;
obj.T=T;
obj.xvars=xvars;
obj.uvars=uvars;
obj.wvars=wvars;
obj.evars=evars;
obj.description='Discrete General Non-Linear model (DGNL)';
obj=class(obj,'dgnl');