From a0ee462fbe457a0e1c273d926f91d157f70445b5 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlsson <fredrikb@nichols.control.lth.se>
Date: Thu, 24 Sep 2015 14:14:13 +0200
Subject: [PATCH] add particle filter implementation
---
pf.m | 35 +++++++++++++++++++++++++++++++++++
track_audio.m | 3 ++-
2 files changed, 37 insertions(+), 1 deletion(-)
create mode 100644 pf.m
diff --git a/pf.m b/pf.m
new file mode 100644
index 0000000..7850828
--- /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 502e3e2..d9ed129 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);
--
GitLab