track_audio.m 3.11 KB
Newer Older
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
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