initial tracking code

track_audio.m

+clear;
+close all;
+clc;
+
+% To be modified
+
+
+%% Simulation parameters
+
+% Parameters used by the auditory front-end
+simParams.fLowHz = 80;          % Lower center frequency of the gammatone
+% filterbank in Hz
+simParams.fHighHz = 8000;       % Upper center frequency of the gammatone
+% filterbank in Hz
+simParams.nChannels = 32;       % Number of filterbank channels
+simParams.frameSize = 50E-3;    % Frame size in seconds
+
+% Optional noise types
+% Choose between 'none', 'busystreet', 'restaurant', 'supermarket' and
+% 'tubestation'
+noiseType = 'tubestation';
+
+% Signal to noise ratio in dB
+snr = 0;
+
+%% 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');
+
+
+%% Simulation
+
+figure(1)
+
+signal = ...
+    % 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;
+
+% Initialize posterior mean and covariance
+posteriorMean = zeros(size(A, 1), length(measuredLocations));
+posteriorCovariance = zeros(size(A, 1), size(A, 1), ...
+    length(measuredLocations));
+
+% Perform localization and tracking
+for l = 1 : length(measuredLocations)
+    % Perform Kalman filter prediction and update
+    [x, P] = kf_predict(x, P, A, Q);
+    [x, P] = kf_update(x, P, measuredLocations(l), 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