Commit fce25fa0 authored by Martin Karlsson's avatar Martin Karlsson
Browse files

Merge branch 'master' of gitlab.control.lth.se:letter2martin/turtlerob1

parents 43b1189c a0ee462f
function get_measurement(dObj)
error('Not implemented yet!')
end
\ No newline at end of file
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
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);
%% Model parameters
Q = [1/4+1e-3, 1/2; 1/2, 1]; % Process noise covariance
R = 1000; % Measurement noise covariance
x = [0; 0]; % Initial state
P = [10, 0; 0, 10]; % Initial state covariance
A = [1, simParams.frameSize; 0, 1]; % System matrix (do not change)
c = [1; 0]; % Output vector (do not change)
% Check definiteness of covariance matrices
if ~all(eig(Q) > 0) || ~all(eig(R) > 0) || ~all(eig(A) > 0)
error('All covariance matrices have to be positive definite.');
end
%% Initialization
% Add necessary paths
addpath('./tools');
addpath('./ekfukf-toolbox');
figure(1)
signal = ... % get signal here
% Get signal length and number of frames
nSamples = size(signal, 1);
nFrames = floor(nSamples / (simParams.frameSize * fsHz));
% Initialize the auditory front-end
simParams.sampleRate = fsHz;
simParams.signalDuration = nSamples / fsHz;
[dataObj, managerObj] = initializeParameters(simParams);
% Get localization measurements
managerObj.processSignal(signal);
azimuthDistribution = ...
squeeze(dataObj.azimuthDistribution{1}.Data(:));
% Use -90 to align coordinate system
measuredLocations = argmax(azimuthDistribution, 2) - 90;
N = length(measuredLocations);
% Initialize posterior mean and covariance
posteriorMean = zeros(size(A, 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
[x, P] = kf_predict(x, P, A, Q);
[x, P] = kf_update(x, P, azimEst, c', R);
posteriorMean(:, l) = x;
posteriorCovariance(:, :, l) = P;
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