track_audio.m 4.24 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
% % % % % %% Request interaural time differences (ITDs)
% % % % % requests = {'itd'};
10
% % % % %
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
11
12
13
14
15
% % % % % % Parameters of the auditory filterbank processor
% % % % % fb_type       = 'gammatone';
% % % % % fb_lowFreqHz  = 80;
% % % % % fb_highFreqHz = 8000;
% % % % % fb_nChannels  = 32;
16
% % % % %
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
17
18
% % % % % % Parameters of innerhaircell processor
% % % % % ihc_method    = 'dau';
19
% % % % %
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
20
21
22
23
% % % % % % Parameters of crosscorrelation processor
% % % % % cc_wSizeSec  = 0.02;
% % % % % cc_hSizeSec  = 0.01;
% % % % % cc_wname     = 'hann';
24
% % % % %
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
% % % % % % 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();
99
    
100
    % Request processing
101
102
    mObj.processSignal(audio);
    azimEst = dObj.localization{1}.Data(end,1); % There might be an issue with several sources here!
103
104
    % Perform Kalman filter prediction and update, TODO: consider changing this
    % crappy filter for a PF
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
105
    [x, P] = kf_predict(x, P, A, Q);
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
106
    [x, P] = kf_update(x, P, azimEst, c', R);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
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
135
    
    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