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
cb1df5f1
Commit
cb1df5f1
authored
Sep 24, 2015
by
Martin Karlsson
Browse files
removed tr
parents
15c9aed6
770f6ebd
Changes
1
Hide whitespace changes
Inline
Side-by-side
track_audio.m
deleted
100644 → 0
View file @
15c9aed6
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
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