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

Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
5
6
7
8
9
10
fs = 24414;
chunks = 2048;
dt = chunks/fs;

[Bf, Af] = butter(3,8000/fs); % filter vectors

Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
11
12
13

%% Setup objects
% Initialize localization models using braodband and subband settings
Martin Karlsson's avatar
Martin Karlsson committed
14
dObj   = dataObject([],fs,10,2);
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
15
16
17
18
19
20
21
22
23
24

% 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
25
26

%% Model parameters
27
sigma_w = 1;
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
28

Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
29
30
31
32
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
x = [0; 0];                                         % Initial state
P = [10, 0; 0, 10];                                 % Initial state covariance
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
33

Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
34
35
A = [1, dt; 0, 1];                                  % System matrix
c = [1; 0];                                         % Output vector
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
36
37
38
39
40
41
42
43
44
45
46
47
48
49

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


50

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
51
52
figure(1)

Martin Karlsson's avatar
Martin Karlsson committed
53
N = 1; % The number of steps to run this stuff.
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
54
55

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

Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
59
60
61
62

% =======================================================
% Main loop - Perform localization and tracking
% =======================================================
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
63
64
tic();
t_old = toc();
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
65
for l = 1:N
66
    audio = get_audio();
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
67
68
    t_new = toc();
    dti = t_new - t_old();
69
    % Request processing
70
71
    mObj.processSignal(audio);
    azimEst = dObj.localization{1}.Data(end,1); % There might be an issue with several sources here!
72
73
    % Perform Kalman filter prediction and update, TODO: consider changing this
    % crappy filter for a PF
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
74
75
76
77
78
    
    
    Qi = [1/4*dti^4+1e-6, 1/2*dti^3; 1/2*dti^3, dti^2]*sigma_w;   % Process noise covariance
    Ai = [1, dti; 0, 1];
    [x, P] = kf_predict(x, P, Ai, Qi);
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
79
    [x, P] = kf_update(x, P, azimEst, c', R);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
80
81
82
    
    posteriorMean(:, l) = x;
    posteriorCovariance(:, :, l) = P;
Martin Karlsson's avatar
Martin Karlsson committed
83
    %pause(max0))
Fredrik Bagge Carlsson's avatar
mhm    
Fredrik Bagge Carlsson committed
84
    t_old = t_new;
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
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