track_audio.m 3.47 KB
Newer Older
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
1
2
3
4
5
6
7
clear;
close all;
clc;

% To be modified


8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
%% 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);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
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



%% 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');


56

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
57
58
figure(1)

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
59
signal = ... % get signal here
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
    % 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;
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
75
N = length(measuredLocations);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
76
77

% Initialize posterior mean and covariance
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
78
79
posteriorMean = zeros(size(A, 1), N);
posteriorCovariance = zeros(size(A, 1), size(A, 1), N);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
80

Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
81
82
83
84

% =======================================================
% Main loop - Perform localization and tracking
% =======================================================
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
85
for l = 1:N
86
87
88
89
90
91
    audio = get_audio();
    dObj = dataObject(audio,fsHz);
    % Create a manager
    mObj = manager(dObj,requests,par);
    % Request processing
    mObj.processSignal();
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
92
    y = get_measurement(dObj);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
93
94
    % Perform Kalman filter prediction and update
    [x, P] = kf_predict(x, P, A, Q);
95
    [x, P] = kf_update(x, P, y, c', R);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
    
    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