pdf.m

particle filter 粒子滤波器 matlab工具箱

function p = pdf(OBJ, t, x)
% PDF  Compute pdf for a Gaussian distribution
%    P = PDF(W, T, X) pdf of W at time T for values X. T is ignored.
  
% Copyright (C) 2005  Gustaf Hendeby
%
% 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.  
  
  % FIXME: Improve input check
  error(nargchk(3, 3, nargin));
  
  if OBJ.n == 1
    xbar = x - OBJ.mu;
    p = (2*pi*OBJ.R)^-.5 * exp(-0.5*xbar./OBJ.R.*xbar);
  else
    % FIXME:  Consider the treatment of this case.
    N = size(x);
    if N(1) ~= OBJ.n || length(N) ~= 2
      error('Cannot compute PDF for arrays of N-dim Gaussians.');
    end
    mu = OBJ.mu; R = OBJ.R;
    for i = 1:N(2)
      xbar = x(:, i) - mu;
      p(i) = exp(-0.5*xbar'/OBJ.R*xbar);
    end
    p = p*(2*pi)^(-.5*OBJ.n)*det(OBJ.R)^(-.5);
  end