diff --git a/pf.m b/pf.m
new file mode 100644
index 0000000000000000000000000000000000000000..78508288467e2aba501e3b9c5ec87f667733c52f
--- /dev/null
+++ b/pf.m
@@ -0,0 +1,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
diff --git a/track_audio.m b/track_audio.m
index 502e3e2a65ba83c5884064e584120a90fcf06fad..d9ed129a56f02d1fed3e65fdbf41cac1543930ac 100644
--- a/track_audio.m
+++ b/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);