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

%% Model parameters
46
47
48
49
fs = 24414;
chunks = 2048;
dt = chunks/fs;
sigma_w = 1;
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
50

51
52
Q = [2/4*dt^4, 1/2*dt^3; 1/2*dt^3, dt^2]*sigma_w;                       % Process noise covariance
R = 1;                                  % Measurement noise covariance
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
53
54
55
x = [0; 0];                             % Initial state
P = [10, 0; 0, 10];                       % Initial state covariance

56
A = [1, dt; 0, 1];     % System matrix (do not change)
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
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');


72

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
73
74
figure(1)

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
75
N = 100;
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
    audio = get_audio();
87
    
88
    % Request processing
89
90
    mObj.processSignal(audio);
    azimEst = dObj.localization{1}.Data(end,1); % There might be an issue with several sources here!
91
92
    % Perform Kalman filter prediction and update, TODO: consider changing this
    % crappy filter for a PF
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
93
    [x, P] = kf_predict(x, P, A, Q);
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
94
    [x, P] = kf_update(x, P, azimEst, c', R);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
95
96
97
    
    posteriorMean(:, l) = x;
    posteriorCovariance(:, :, l) = P;
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
98
    pause(0.5)
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
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
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