track_audio.m 3.15 KB
Newer Older
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
1
2
3
4
5
6
7
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
clear;
close all;
clc;

% To be modified


%% Simulation parameters

% Parameters used by the auditory front-end
simParams.fLowHz = 80;          % Lower center frequency of the gammatone
% filterbank in Hz
simParams.fHighHz = 8000;       % Upper center frequency of the gammatone
% filterbank in Hz
simParams.nChannels = 32;       % Number of filterbank channels
simParams.frameSize = 50E-3;    % Frame size in seconds


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


figure(1)

Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
44
signal = ... % get signal here
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
    % 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
60
N = length(measuredLocations);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
61
62

% Initialize posterior mean and covariance
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
63
64
posteriorMean = zeros(size(A, 1), N);
posteriorCovariance = zeros(size(A, 1), size(A, 1), N);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
65
66

% Perform localization and tracking
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
67
68
69
for l = 1:N
    
    % ===================================================
70
    % TODO: Perform steps to get measurements inside this loop!
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
71
    % ===================================================
72
73
%     y = get_measurement();
    y = measuredLocations(l); % TODO: Modify this to get a measurement!
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
74
75
    % Perform Kalman filter prediction and update
    [x, P] = kf_predict(x, P, A, Q);
76
    [x, P] = kf_update(x, P, y, c', R);
Fredrik Bagge Carlsson's avatar
Fredrik Bagge Carlsson committed
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
    
    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