Commit a0ee462f authored by Fredrik Bagge Carlsson's avatar Fredrik Bagge Carlsson
Browse files

add particle filter implementation

parent d00ca4b5
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
......@@ -100,7 +100,8 @@ for l = 1:N
% Request processing
mObj.processSignal(audio);
azimEst = dObj.localization{1}.Data(end,1); % There might be an issue with several sources here!
% Perform Kalman filter prediction and update
% Perform Kalman filter prediction and update, TODO: consider changing this
% crappy filter for a PF
[x, P] = kf_predict(x, P, A, Q);
[x, P] = kf_update(x, P, azimEst, c', R);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment