diff --git a/track_audio.m b/track_audio.m new file mode 100644 index 0000000000000000000000000000000000000000..7b77e66f7c373da3a5a12303a143a07a3d55055d --- /dev/null +++ b/track_audio.m @@ -0,0 +1,108 @@ +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