add particle filter implementation

pf.m

+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

track_audio.m

@@ -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);