danl.m

particle filter 粒子滤波器 matlab工具箱

function obj = pfsys(varargin)
% Constructor for the DANL (Discrete Additive Non-Linear) model
%
% x(t+T) = f(x,t) + gu(x,t)*u(t) + gw(x,t)*w(t)
%   y(t) = h(x,t) + hu(x,t)*u(t) + e(t)
%
% Syntax: (* = optional)
%
% model = danl(f, gw, gu, h, hu, x0, w, e, p0*, T*, xvars*);
% model = danl(ltiobj);
%
% 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 expression object.
%	Note: if a double is given, a xlinear object will be created with 'evalvar' set to 1,
%	i.e. the matrix will be multiplied with x when evaluating f(x,t).
% 1. ltiobj
%	A LTI object (ss, tf or zpk), from the control toolbox. 
%	Necessary data will be extracted, and normally distributed noise with variance
%	1 will be used.
% 2. gw
%	Data object containing gw
%	If a double (matrix), or a cell array of strings (or inlines) is given, it will be
%	used to create an apropriate expression object.
%	Note: if a double is given, a xlinear object will be created with 'evalvar' set to 0,
%	i.e. gw(x,t) is constant and does NOT depend on x.
% 3. gu
%	Data object containing gu
%	If a double (matrix), or a cell array of strings (or inlines) is given, it will be
%	used to create an apropriate data object.
%	Note: if a double is given, a xlinear object will be created with 'evalvar' set to 0,
%	ie gu(x,t) is constant and does NOT depend on x.
% 4. 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.
%	Note: if a double is given, a xlinear object will be created with 'evalvar' set to 1,
%	ie the matrix will be multiplied with x when evaluating h(x,t).
% 5. hu
%	Data object containing hu
%	If a double (matrix), or a cell array of strings (or inlines) is given, it will be
%	used to create an apropriate data object.
%	Note: if a double is given, a xlinear object will be created with 'evalvar' set to 0,
%	ie hu(x,t) is constant and does NOT depend on x.
% 6. x0
%	A column vector containing the expectation of the initial state estimate.
%	Each row represents a state.
% 7. 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.
% 8. 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.
% 9* 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.
% 9* []
%	'p0' is set to independent gaussian noise with variance 1.
% 10* T
%	Sample time.
% 10* []
%	'T' is set to 1.
% 11* xvars
%	A cell array containing the names of the states.
% 11* []
%	'xvars' is set to {'x1', 'x2', 'x3', ...}
%
% Out arguments:
%
% 1. model
%	The resulting danl 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.gw=[];
	obj.gu=[];
	obj.h=[];
	obj.hu=[];
	obj.x0=[];
	obj.w=[];
	obj.e=[];
	obj.p0=[];
	obj.T=1;
	obj.xvars={};
	obj.description='Discrete Additive Non-Linear model (DANL)';
	obj=class(obj,'danl');
	return;				%Exit
elseif nargin==1
	argin=varargin{1};
	if isa(argin,'danl')
		% Copy constructor
		obj = argin;		%Copy
		return;			%Exit
	elseif isa(argin,'ss')||isa(argin,'tf')||isa(argin,'zpk')
		% An LTI object was given
		obj=lti2danl(argin);	%Convert
		return;			%Exit
	end
end;

error(nargchk(8, 11, nargin));

% Fetch gw
gwobj=varargin{2};
if isempty(gwobj);
	% Gw was empty. Set it to 1 (we can't just leave it out)
	gwobj=1;
end;

% Fetch x0
x0=varargin{6};
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={};

% Fetch arguments, if they exist
if nargin>=9; p0temp=varargin{9}; end;
if nargin>=10; T=varargin{10}; end;
if nargin>=11; xvars=varargin{11};  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
	states=length(x0);
	xvars=cell(1,states);
	for i=1:states
		xvars{i}=['x',num2str(i)];
	end
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{7});
e=initnoise(varargin{8});

% Initialize data. If xvars={}, default xvars will be calculated
f=initdata(varargin{1},xvars,{},{},1);
gw=initdata(gwobj,xvars,{},{},0);
gu=initdata(varargin{3},xvars,{},{},0);
h=initdata(varargin{4},xvars,{},{},1);
hu=initdata(varargin{5},xvars,{},{},0);

%Initialize object data
obj.f=f;
obj.gw=gw;
obj.gu=gu;
obj.h=h;
obj.hu=hu;
obj.x0=x0;
obj.w=w;
obj.e=e;
obj.p0=p0;
obj.T=T;
obj.xvars=xvars;
obj.description='Discrete Additive Non-Linear model (DANL)';
obj=class(obj,'danl');