pf.m 616 Bytes
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
function [xp, w, expw] = pf( y, xp, w, expw, g_density, f)
% Particle filter for sound source tracking

N = length(xp);

% Resample
if(1/sum(expw.^2) < N/2)
    resampleInstants(t) = 1;
    %         [~, j] = histc(rand(N,1), [0 cumsum(exp(w(:,t-1))')]);
    bins = [0 cumsum(expw')];
    
    bins = bins./(bins(end));
    [~, j] = histc((rand(1)/N+0):1/N:1, bins);
    
    
    xp = xp(j);
    w = log(1/N)*ones(N,1);
else
    
end

% Time update
xp = f(xp);
w = w + g_density(y,xp);

offset = max(w);
normConstant = log(sum(exp(w-offset)))+offset;

w = w-normConstant; % Normalize weights


expw = exp(w);


end