Commit a57a8cae authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

Preparations for particle filtering

parent 95ea3295
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
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 % Particle filter for sound source tracking
N = length(xp); N = length(xp);
% Resample % Resample
if(1/sum(expw.^2) < N/2) 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 = [0 cumsum(expw')];
bins = bins./(bins(end)); bins = bins./(bins(end));
[~, j] = histc((rand(1)/N+0):1/N:1, bins); [~, j] = histc((rand(1)/N+0):1/N:1, bins);
xp = xp(:,j);
xp = xp(j);
w = log(1/N)*ones(N,1); w = log(1/N)*ones(N,1);
else
end end
% Time update % Time update
xp = f(xp); xp = f(xp);
w = w + g_density(y,xp); w = w + g_density(y,xp,xr);
offset = max(w); offset = max(w);
normConstant = log(sum(exp(w-offset)))+offset; normConstant = log(sum(exp(w-offset)))+offset;
......
...@@ -45,18 +45,15 @@ mObj = manager(dObj,'localization',par_sub); ...@@ -45,18 +45,15 @@ mObj = manager(dObj,'localization',par_sub);
%% Particle filter parameters %% Particle filter parameters
sigma_w = 1; % State noise std sigma_w = 1; % State noise std
sigma_v = 1; % Measurement noise std sigma_v = 1; % Measurement noise std
sigma_w0 = 10;
fs = 24414; fs = 24414;
chunks = 2048; chunks = 2048;
dt = chunks/fs; dt = chunks/fs;
Q = [1/4*dt^4, 1/2*dt^3; 1/2*dt^3, dt^2]*sigma_w; % Process noise covariance Npart = 500;
R = 1; % Measurement noise covariance xp = sigma_w0*randn(2,Npart);
x = [0; 0]; % Initial state 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
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
%% Initialization %% Initialization
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment