calc_x0.m

particle filter 粒子滤波器 matlab工具箱

function v=calc_x0(obj,N);
% Calculates the stochastic initital state estimate, x0
%
% Syntax: (* = optional)
%
% x0 = calc_x0(model, N);
%
% In arguments:
%
% 1. model
%	Model object
% 2. N
%	Amount of x0 estimates. Each estimate is represented by a column in the resulting
%	matrix, ie x0(i,k) represents the initial state i of estimate k.
%	Useful for particle filter where you need initial estimates for each particle.
%	For algorithms that doesn't utilize particles, such as the EKF algorithm, 'N'
%	is usually set to 1.
%
% Out arguments:
%
% 1. x0
%	The stochastic initial state estimate

% 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.

% Get the NON-STOCHASTIC state estimate from the model object
x0=obj.x0;

% Draw random data from the p0 object
if isempty(obj.p0)
	% No p0 exists. Don't add any noise.
	% This situation is not possible in the current system, but is included for
	% consistency.
	p0=0;
else
	% We have a p0 object. Draw from it!
	p0=random(obj.p0,0,1,N);
end;

% Now add the random data to the x0 scalar taken from the model object.
if length(x0)>1
	% x0 is a vector. We need to use repmat to make the dimensions match.
	v=repmat(x0,1,N)+p0;
else
	% x0 is a scalar. There are no problems with the dimensions, just add!
	v=x0+p0;
end