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
Fredrik Bagge Carlson
SystemIdentification
Commits
cbb84401
Commit
cbb84401
authored
Sep 19, 2015
by
Fredrik Bagge Carlson
Browse files
updates to particle filters, speedy performance improvements
parent
913bb57e
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/particle_filters/particle_filter_test.jl
View file @
cbb84401
...
...
@@ -21,6 +21,13 @@ function f(x::Vector, t::Int64)
end
f
(
x
::
Float64
,
t
::
Int64
)
=
0.5
*
x
+
25
*
x
/
(
1
+
x
^
2
)
+
8
*
cos
(
1.2
*
(
t
-
1
))
f
(
xn
,
x
,
t
::
Int64
)
=
begin
c
=
8
*
cos
(
1.2
*
(
t
-
1
))
@inbounds
for
i
=
1
:
length
(
x
)
xn
[
i
]
=
0.5
*
x
[
i
]
+
25
*
x
[
i
]
./
(
1
+
x
[
i
]
^
2
)
+
c
+
σw
*
randn
()
end
end
function
f_sample
(
x
::
Vector
,
t
::
Int64
)
c
=
8
*
cos
(
1.2
*
(
t
-
1
))
@inbounds
for
i
=
1
:
length
(
x
)
...
...
src/particle_filters/pf_bootstrap.jl
View file @
cbb84401
# module Tmp
using
Devectorize
using
StatsBase
function
pf_bootstrap!
(
xp
,
w
,
y
,
N
,
g_density
,
f
)
T
=
length
(
y
)
w0
=
fill
(
log
(
1
/
N
),
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
)
,
1
)
w1
=
copy
(
w0
)
g_density
(
y
[
1
],
xp
[
:
,
1
],
w1
)
w
[
:
,
1
]
=
w1
@devec
w
[
:
,
1
]
-=
log
(
sum
(
exp
(
w
[
:
,
1
]))
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
wT
=
slice
(
w
,
:
,
1
)
xT
=
slice
(
xp
,
:
,
1
)
fill!
(
wT
,
log
(
1
/
N
)
)
g
(
y
[
1
],
xT
,
wT
)
wT
-=
logsumexp
(
w
T
)
j
=
Array
(
Int64
,
N
)
# tw = Float64(N)
for
t
=
2
:
T
xT1
=
xT
xT
=
slice
(
xp
,
:
,
t
)
wT1
=
wT
wT
=
slice
(
w
,
:
,
t
)
# Resample
if
t
%
2
==
0
resample_systematic!
(
j
,
slice
(
w
,
:
,
t
-
1
)
,
N
)
xpT
=
xp
[
j
,
t
-
1
]
wT
=
copy
(
w0
)
resample_systematic!
(
j
,
wT1
,
N
)
f
(
xT
,
xT1
[
j
]
,
t
-
1
)
fill!
(
wT
,
log
(
1
/
N
)
)
else
# Resample not needed
xpT
=
xp
[
:
,
t
-
1
]
wT
=
w
[
:
,
t
-
1
]
f
(
xT
,
xT1
,
t
-
1
)
copy!
(
wT
,
wT1
)
end
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
g_density
(
y
[
t
],
xp
[
:
,
t
],
wT
)
w
[
:
,
t
]
=
wT
g
(
y
[
t
],
xT
,
wT
)
# Normalize weights
offset
=
maximum
(
w
[
:
,
t
])
normConstant
=
0.0
for
i
=
1
:
N
normConstant
+=
exp
(
w
[
i
,
t
]
-
offset
)
end
w
[
:
,
t
]
-=
log
(
normConstant
)
+
offset
wT
-=
logsumexp
(
wT
)
# tw = 1/sum(w(:,t).*2)
end
...
...
@@ -44,38 +41,34 @@ function pf_bootstrap(y, N, g_density, f)
T
=
length
(
y
)
xp
=
Array
(
Float64
,(
N
,
T
))
w
=
Array
(
Float64
,(
N
,
T
))
w0
=
fill
(
log
(
1
/
N
),
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
)
,
1
)
w1
=
copy
(
w0
)
g_density
(
y
[
1
],
xp
[
:
,
1
],
w1
)
w
[
:
,
1
]
=
w1
@devec
w
[
:
,
1
]
-=
log
(
sum
(
exp
(
w
[
:
,
1
]))
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
wT
=
slice
(
w
,
:
,
1
)
xT
=
slice
(
xp
,
:
,
1
)
fill!
(
wT
,
log
(
1
/
N
)
)
g
(
y
[
1
],
xT
,
wT
)
wT
-=
logsumexp
(
w
T
)
j
=
Array
(
Int64
,
N
)
# tw = Float64(N)
for
t
=
2
:
T
xT1
=
xT
xT
=
slice
(
xp
,
:
,
t
)
wT1
=
wT
wT
=
slice
(
w
,
:
,
t
)
# Resample
if
t
%
2
==
0
resample_systematic!
(
j
,
slice
(
w
,
:
,
t
-
1
)
,
N
)
xpT
=
xp
[
j
,
t
-
1
]
wT
=
copy
(
w0
)
resample_systematic!
(
j
,
wT1
,
N
)
f
(
xT
,
xT1
[
j
]
,
t
-
1
)
fill!
(
wT
,
log
(
1
/
N
)
)
else
# Resample not needed
xpT
=
xp
[
:
,
t
-
1
]
wT
=
w
[
:
,
t
-
1
]
f
(
xT
,
xT1
,
t
-
1
)
copy!
(
wT
,
wT1
)
end
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
g_density
(
y
[
t
],
xp
[
:
,
t
],
wT
)
w
[
:
,
t
]
=
wT
g
(
y
[
t
],
xT
,
wT
)
# Normalize weights
offset
=
maximum
(
w
[
:
,
t
])
normConstant
=
0.0
for
i
=
1
:
N
normConstant
+=
exp
(
w
[
i
,
t
]
-
offset
)
end
w
[
:
,
t
]
-=
log
(
normConstant
)
+
offset
wT
-=
logsumexp
(
wT
)
# tw = 1/sum(w(:,t).*2)
end
...
...
@@ -87,42 +80,37 @@ end
function
pf_bootstrap_nn
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
)
T
=
length
(
y
)
w
0
=
fill
(
log
(
1
/
N
),
N
)
x
p
[
:
,
1
]
=
f
(
zeros
(
N
)
,
1
)
w1
=
copy
(
w0
)
g
_density
(
y
[
1
],
x
p
[
:
,
1
]
,
w
1
)
wnn
[
:
,
1
]
=
w
1
@devec
w
[
:
,
1
]
=
wnn
[
:
,
1
]
-
log
(
sum
(
exp
(
w
nn
[
:
,
1
]))
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
w
T
=
slice
(
w
,
:
,
1
)
x
T
=
slice
(
xp
,
:
,
1
)
fill!
(
wT
,
log
(
1
/
N
)
)
g
(
y
[
1
],
x
T
,
w
T
)
wnn
[
:
,
t
]
=
w
T
wT
-=
logsumexp
(
w
T
)
j
=
Array
(
Int64
,
N
)
# tw = Float64(N)
for
t
=
2
:
T
xT1
=
xT
xT
=
slice
(
xp
,
:
,
t
)
wT1
=
wT
wT
=
slice
(
w
,
:
,
t
)
# Resample
if
t
%
2
==
0
resample_systematic!
(
j
,
w
[
:
,
t
-
1
],
N
)
# resample_systematic!(j,slice(w,:,t-1),N)
xpT
=
xp
[
j
,
t
-
1
]
wT
=
copy
(
w0
)
resample_systematic!
(
j
,
wT1
,
N
)
f
(
xT
,
xT1
[
j
],
t
-
1
)
fill!
(
wT
,
log
(
1
/
N
))
else
# Resample not needed
xpT
=
xp
[
:
,
t
-
1
]
wT
=
copy
(
w
[
:
,
t
-
1
]
)
f
(
xT
,
xT1
,
t
-
1
)
copy
!
(
w
T
,
wT1
)
end
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
g_density
(
y
[
t
],
xp
[
:
,
t
],
wT
)
wnn
[
:
,
t
]
=
wT
# Normalize weights
#some strange error with this normalization method, I totally have no clue why it's working elsewhere but not here
offset
=
maximum
(
wnn
[
:
,
t
])
normConstant
=
0.0
for
i
=
1
:
N
normConstant
+=
exp
(
wnn
[
i
,
t
]
-
offset
)
end
nc
=
log
(
normConstant
)
+
offset
w
[
:
,
t
]
=
wnn
[
:
,
t
]
-
nc
# @assert isapprox(nc,log(sum(exp(wnn[:,t]))))
wT
-=
logsumexp
(
wT
)
# @assert isapprox(nc,log(sum(exp(wnn[:,t]))))
# tw = 1/sum(w(:,t).*2)
end
...
...
@@ -132,7 +120,7 @@ end
function
pf_aux_nn
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
)
T
=
length
(
y
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
w0
=
fill
(
log
(
1
/
N
),
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
),
1
)
wnn
[
:
,
1
]
=
w0
+
g_density
(
y
[
1
],
xp
[
:
,
1
])
...
...
@@ -184,41 +172,36 @@ end
function
pf_CSMC!
(
xp
,
w
,
y
,
N
,
g_density
,
f
,
xc
)
T
=
length
(
y
)
w0
=
fill
(
log
(
1
/
N
),
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
),
1
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
xp
[
N
,
1
]
=
xc
[
1
]
w1
=
copy
(
w0
)
g_density
(
y
[
1
],
xp
[
:
,
1
],
w1
)
w
[
:
,
1
]
=
w1
@devec
w
[
:
,
1
]
-=
log
(
sum
(
exp
(
w
[
:
,
1
])))
T
=
length
(
y
)
wT
=
slice
(
w
,
:
,
1
)
xT
=
slice
(
xp
,
:
,
1
)
fill!
(
wT
,
log
(
1
/
N
))
g
(
y
[
1
],
xT
,
wT
)
wT
-=
logsumexp
(
wT
)
j
=
Array
(
Int64
,
N
)
# tw = Float64(N)
for
t
=
2
:
T
xT1
=
xT
xT
=
slice
(
xp
,
:
,
t
)
wT1
=
wT
wT
=
slice
(
w
,
:
,
t
)
# Resample
if
t
%
2
==
0
resample_systematic!
(
j
,
slice
(
w
,
:
,
t
-
1
)
,
N
)
xpT
=
xp
[
j
,
t
-
1
]
wT
=
copy
(
w0
)
resample_systematic!
(
j
,
wT1
,
N
)
f
(
xT
,
xT1
[
j
]
,
t
-
1
)
fill!
(
wT
,
log
(
1
/
N
)
)
else
# Resample not needed
xpT
=
xp
[
:
,
t
-
1
]
wT
=
w
[
:
,
t
-
1
]
f
(
xT
,
xT1
,
t
-
1
)
copy!
(
wT
,
wT1
)
end
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
xp
[
N
,
t
]
=
xc
[
t
]
g_density
(
y
[
t
],
xp
[
:
,
t
],
wT
)
w
[
:
,
t
]
=
wT
# Time update
g
(
y
[
t
],
xT
,
wT
)
# Normalize weights
offset
=
maximum
(
w
[
:
,
t
])
normConstant
=
0.0
for
i
=
1
:
N
normConstant
+=
exp
(
w
[
i
,
t
]
-
offset
)
end
w
[
:
,
t
]
-=
log
(
normConstant
)
+
offset
wT
-=
logsumexp
(
wT
)
# tw = 1/sum(w(:,t).*2)
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