From a57a8cae6ffe3bdc0102e322003ef1a6afed3890 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <fredrikb@control.lth.se>
Date: Fri, 25 Sep 2015 10:19:44 +0200
Subject: [PATCH] Preparations for particle filtering
---
g_density.m | 8 ++++++++
pf.m | 13 +++----------
track_audio_pf.m | 13 +++++--------
3 files changed, 16 insertions(+), 18 deletions(-)
create mode 100644 g_density.m
diff --git a/g_density.m b/g_density.m
new file mode 100644
index 0000000..def884e
--- /dev/null
+++ b/g_density.m
@@ -0,0 +1,8 @@
+function r = g_density(y,xp,xr)
+sigma_v = 1;
+N = length(xp);
+v = xp - repmat(xr,1,N);
+az = atan2d(v(2,:),v(1,:));
+
+r = -0.5/sigma_v * (y-az).^2; %% This assumes gaussian noise, try to change to Laplacian noise for robustness
+end
\ No newline at end of file
diff --git a/pf.m b/pf.m
index 7850828..be40b21 100644
--- a/pf.m
+++ b/pf.m
@@ -1,27 +1,20 @@
-function [xp, w, expw] = pf( y, xp, w, expw, g_density, f)
+function [xp, w, expw] = pf(xr, 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);
+ xp = xp(:,j);
w = log(1/N)*ones(N,1);
-else
-
end
% Time update
xp = f(xp);
-w = w + g_density(y,xp);
+w = w + g_density(y,xp,xr);
offset = max(w);
normConstant = log(sum(exp(w-offset)))+offset;
diff --git a/track_audio_pf.m b/track_audio_pf.m
index 3fa60b5..3d11615 100644
--- a/track_audio_pf.m
+++ b/track_audio_pf.m
@@ -45,18 +45,15 @@ mObj = manager(dObj,'localization',par_sub);
%% Particle filter parameters
sigma_w = 1; % State noise std
sigma_v = 1; % Measurement noise std
+sigma_w0 = 10;
fs = 24414;
chunks = 2048;
dt = chunks/fs;
-Q = [1/4*dt^4, 1/2*dt^3; 1/2*dt^3, dt^2]*sigma_w; % Process noise covariance
-R = 1; % Measurement noise covariance
-x = [0; 0]; % Initial state
-
-A = [1, dt; 0, 1];
-Npart = 100;
-f = @(x) A*x + sigma_w*diag(Q)*randn(1,size(x,2));
-g_density = @(y,x) -0.5/sigma_v * (y-x(1,:)).^2; %% This assumes gaussian noise, try to change to Laplacian noise for robustness
+Npart = 500;
+xp = sigma_w0*randn(2,Npart);
+f = @(x) x + sigma_w*randn(size(x));
+% g_density = @(y,x) -0.5/sigma_v * (y-x(1,:)).^2; %% This assumes gaussian noise, try to change to Laplacian noise for robustness
%% Initialization
--
GitLab