track_audio.m 4.17 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


Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
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
% % % % % %% 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);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67

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


68

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
69
70
figure(1)

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
71
signal = ... % get signal here
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
    % 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
87
N = length(measuredLocations);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
88
89

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

Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
93
94
95
96

% =======================================================
% Main loop - Perform localization and tracking
% =======================================================
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
97
for l = 1:N
98
    audio = get_audio();
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
99
    % TODO: Put the audio in the data object!
100
101
    % Request processing
    mObj.processSignal();
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
102
    azimEst = dObj_sub.localization{1}.Data(end);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
103
104
    % Perform Kalman filter prediction and update
    [x, P] = kf_predict(x, P, A, Q);
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
105
    [x, P] = kf_update(x, P, azimEst, c', R);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
    
    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