Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Martin Karlsson
turtlerob1
Commits
fce25fa0
Commit
fce25fa0
authored
Sep 24, 2015
by
Martin Karlsson
Browse files
Merge branch 'master' of gitlab.control.lth.se:letter2martin/turtlerob1
parents
43b1189c
a0ee462f
Changes
3
Hide whitespace changes
Inline
Side-by-side
get_measurement.m
0 → 100644
View file @
fce25fa0
function get_measurement(dObj)
error('Not implemented yet!')
end
\ No newline at end of file
pf.m
0 → 100644
View file @
fce25fa0
function
[
xp
,
w
,
expw
]
=
pf
(
y
,
xp
,
w
,
expw
,
g_density
,
f
)
% Particle filter for sound source tracking
N
=
length
(
xp
);
% Resample
if
(
1
/
sum
(
expw
.^
2
)
<
N
/
2
)
resampleInstants
(
t
)
=
1
;
% [~, j] = histc(rand(N,1), [0 cumsum(exp(w(:,t-1))')]);
bins
=
[
0
cumsum
(
expw
'
)];
bins
=
bins
.
/(
bins
(
end
));
[
~
,
j
]
=
histc
((
rand
(
1
)/
N
+
0
):
1
/
N
:
1
,
bins
);
xp
=
xp
(
j
);
w
=
log
(
1
/
N
)
*
ones
(
N
,
1
);
else
end
% Time update
xp
=
f
(
xp
);
w
=
w
+
g_density
(
y
,
xp
);
offset
=
max
(
w
);
normConstant
=
log
(
sum
(
exp
(
w
-
offset
)))
+
offset
;
w
=
w
-
normConstant
;
% Normalize weights
expw
=
exp
(
w
);
end
track_audio.m
0 → 100644
View file @
fce25fa0
clear
;
close
all
;
clc
;
% To be modified
% % % % % %% Request interaural time differences (ITDs)
% % % % % requests = {'itd'};
% % % % %
% % % % % % Parameters of the auditory filterbank processor
% % % % % fb_type = 'gammatone';
% % % % % fb_lowFreqHz = 80;
% % % % % fb_highFreqHz = 8000;
% % % % % fb_nChannels = 32;
% % % % %
% % % % % % Parameters of innerhaircell processor
% % % % % ihc_method = 'dau';
% % % % %
% % % % % % Parameters of crosscorrelation processor
% % % % % cc_wSizeSec = 0.02;
% % % % % cc_hSizeSec = 0.01;
% % % % % cc_wname = 'hann';
% % % % %
% % % % % % Summary of parameters
% % % % % par = genParStruct('fb_type',fb_type,'fb_lowFreqHz',fb_lowFreqHz,...
% % % % % 'fb_highFreqHz',fb_highFreqHz,'fb_nChannels',fb_nChannels,...
% % % % % 'ihc_method',ihc_method,'cc_wSizeSec',cc_wSizeSec,...
% % % % % 'cc_hSizeSec',cc_hSizeSec,'cc_wname',cc_wname);
%% Setup objects
% Initialize localization models using braodband and subband settings
dObj
=
dataObject
([],
fsHz
,
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
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
)
signal
=
...
% get signal here
% 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
;
N
=
length
(
measuredLocations
);
% 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
% =======================================================
for
l
=
1
:
N
audio
=
get_audio
();
% 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
[
x
,
P
]
=
kf_predict
(
x
,
P
,
A
,
Q
);
[
x
,
P
]
=
kf_update
(
x
,
P
,
azimEst
,
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
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment