Preparations for particle filtering

g_density.m

+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

pf.m

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

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