observe.m

来自「this code include particle filter techni」· M 代码 · 共 25 行

function w=observe(x,data,sigma,target_hist,Hx,Hy)
N = size(x,1);
[nd md] = size(data(:,:,1));
rho = repmat(0,N,1);
for i=1:N
    wr = [max(1,uint8(x(i,2)-Hy)):min(uint8(x(i,2)+Hy),nd)];
    wc = [max(1,uint8(x(i,1)-Hx)):min(uint8(x(i,1)+Hx),md)];
    win = data(wr, wc, :);
    a=size(win);
    if(a(1)*a(2)~=0)
        data_hist = ...
            [imhist(win(:,:,1),8)' imhist(win(:,:,2),8)' imhist(win(:,:,3),8)'];
        data_hist=data_hist./sum(data_hist);
        rho(i,:) = sum(sqrt(data_hist.*target_hist),2);
    end
end
%target_hist = repmat(target_hist,N,1);
%rho = sum(sqrt(data_hist.*target_hist),2);
w=exp((rho-1)/(2*sigma*sigma))/(sigma*sqrt(2*pi));
w=w./sum(w);