Commit f255763a authored by Fredrik Bagge Carlsson's avatar Fredrik Bagge Carlsson
Browse files

initial tracking code

parent b211e423
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
% Optional noise types
% Choose between 'none', 'busystreet', 'restaurant', 'supermarket' and
% 'tubestation'
noiseType = 'tubestation';
% Signal to noise ratio in dB
snr = 0;
%% 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');
%% Simulation
figure(1)
signal = ...
% 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;
% Initialize posterior mean and covariance
posteriorMean = zeros(size(A, 1), length(measuredLocations));
posteriorCovariance = zeros(size(A, 1), size(A, 1), ...
length(measuredLocations));
% Perform localization and tracking
for l = 1 : length(measuredLocations)
% Perform Kalman filter prediction and update
[x, P] = kf_predict(x, P, A, Q);
[x, P] = kf_update(x, P, measuredLocations(l), c', R);
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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment