From c63be83a1f108f1c06365e28d226605c46d6a5d3 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlsson <fredrikb@nichols.control.lth.se>
Date: Thu, 24 Sep 2015 16:58:46 +0200
Subject: [PATCH] tr back
---
track_audio.m | 110 ++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 110 insertions(+)
create mode 100644 track_audio.m
diff --git a/track_audio.m b/track_audio.m
new file mode 100644
index 0000000..97bf40c
--- /dev/null
+++ b/track_audio.m
@@ -0,0 +1,110 @@
+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
--
GitLab