tr back

track_audio.m

+clear;
+close all;
+clc;
+
+fs = 24414;
+chunks = 2048;
+dt = chunks/fs;
+
+[Bf, Af] = butter(3,8000/fs); % filter vectors
+
+
+%% Setup objects
+% Initialize localization models using braodband and subband settings
+dObj   = dataObject([],fs,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);
+
+%% Model parameters
+sigma_w = 1;
+
+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
+
+A = [1, dt; 0, 1];                                  % System matrix
+c = [1; 0];                                         % Output vector
+
+% 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)
+
+N = 1; % The number of steps to run this stuff.
+
+% Initialize posterior mean and covariance
+posteriorMean = zeros(size(A, 1), N);
+posteriorCovariance = zeros(size(A, 1), size(A, 1), N);
+
+
+% =======================================================
+% Main loop - Perform localization and tracking
+% =======================================================
+tic();
+t_old = toc();
+for l = 1:N
+    audio = get_audio();
+    t_new = toc();
+    dti = t_new - t_old();
+    % Request processing
+    mObj.processSignal(audio);
+    azimEst = dObj.localization{1}.Data(end,1); % There might be an issue with several sources here!
+    % Perform Kalman filter prediction and update, TODO: consider changing this
+    % crappy filter for a PF
+    
+    
+    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);
+    [x, P] = kf_update(x, P, azimEst, c', R);
+    
+    posteriorMean(:, l) = x;
+    posteriorCovariance(:, :, l) = P;
+    %pause(max0))
+    t_old = t_new;
+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