Skip to content
GitLab
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
a0ee462f
Commit
a0ee462f
authored
Sep 24, 2015
by
Fredrik Bagge Carlsson
Browse files
add particle filter implementation
parent
d00ca4b5
Changes
2
Hide whitespace changes
Inline
Side-by-side
pf.m
0 → 100644
View file @
a0ee462f
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
View file @
a0ee462f
...
...
@@ -100,7 +100,8 @@ for l = 1:N
% 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
% 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
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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