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

get_measurement.m

+function get_measurement(dObj)
+error('Not implemented yet!')
+
+end 
\ No newline at end of file

pf.m

+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

track_audio.m

+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