Commit f60695be authored by Fredrik Bagge Carlsson's avatar Fredrik Bagge Carlsson
Browse files

added particle filter tracking alorithm

parent fb7315cd
clear;
close all;
clc;
% To be modified
% % % % % %% Request interaural time differences (ITDs)
% % % % % requests = {'itd'};
% % % % %
% % % % % % Parameters of the auditory filterbank processor
% % % % % fb_type = 'gammatone';
% % % % % fb_lowFreqHz = 80;
% % % % % fb_highFreqHz = 8000;
% % % % % fb_nChannels = 32;
% % % % %
% % % % % % Parameters of innerhaircell processor
% % % % % ihc_method = 'dau';
% % % % %
% % % % % % Parameters of crosscorrelation processor
% % % % % cc_wSizeSec = 0.02;
% % % % % cc_hSizeSec = 0.01;
% % % % % cc_wname = 'hann';
% % % % %
% % % % % % Summary of parameters
% % % % % par = genParStruct('fb_type',fb_type,'fb_lowFreqHz',fb_lowFreqHz,...
% % % % % 'fb_highFreqHz',fb_highFreqHz,'fb_nChannels',fb_nChannels,...
% % % % % 'ihc_method',ihc_method,'cc_wSizeSec',cc_wSizeSec,...
% % % % % 'cc_hSizeSec',cc_hSizeSec,'cc_wname',cc_wname);
%% Setup objects
% Initialize localization models using braodband and subband settings
dObj = dataObject([],fsHz,10,2);
% Settings for subband approach
par_sub = genParStruct('cc_bBroadband',0,'cc_wSizeSec',winSec,...
'cc_hSizeSec',winSec/2,'cc_maxDelaySec',1.25e-3,...
'fb_lowFreqHz',fLowHz,'fb_highFreqHz',fHighHz,...
'fb_nERBs',1,'ihc_method','none',...
'loc_NSources',nSpeakers(hh));
% Initialize localization models using braodband and subband settings
mObj = manager(dObj,'localization',par_sub);
%% Particle filter parameters
sigma_w = 1; % State noise std
sigma_v = 1; % Measurement noise std
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
%% Initialization
figure(1)
N = 100;
% Initialize posterior mean and covariance
posteriorMean = zeros(1, N);
posteriorCovariance = zeros(size(A, 1), size(A, 1), N);
% =======================================================
% Main loop - Perform localization and tracking
% =======================================================
for l = 1:N
audio = get_audio();
% 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, TODO: consider changing this
% crappy filter for a PF
[xp, w, expw] = pf( azimEst, xp, w, expw, g_density, f);
% [~,I] = max(expw);
xh = sum(xp(1,:).*expw);
posteriorMean(l) = xh;
end
% Plot measurements
subplot(2, nFiles / 2, k);
timeAxis = linspace(0, nSamples / fsHz, nFrames);
plot(timeAxis, measuredLocations, 'x', 'LineWidth', 2);
axis([0, nSamples / fsHz, -90, 90]);
xlabel('Time / s');
ylabel('Azimuth / deg');
grid on; hold on;
plot(timeAxis, posteriorMean(1, :), 'g', 'LineWidth', 2);
% Plot ground truth
plot(timeAxis, gtTrajectory, 'r--', 'LineWidth', 2);
legend('Measurements', 'Estimated trajectory', 'Ground truth');
% Compute RMSE
rmse = sqrt(sum((posteriorMean(1, :) - gtTrajectory).^2) ./ nFrames);
if ~strcmpi(noiseType, 'none')
title([upper(soundType), ', ', upper(noiseType), ' NOISE AT ', ...
num2str(snr), ' dB SNR, ', 'RMSE: ', num2str(rmse), '°']);
else
title([upper(soundType), ', NO NOISE, ', 'RMSE: ', ...
num2str(rmse), '°']);
end
Markdown is supported
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