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
913bb57e
Commit
913bb57e
authored
Sep 16, 2015
by
Fredrik Bagge Carlson
Browse files
Huge update to all particle filter files
parent
6695ecac
Changes
7
Hide whitespace changes
Inline
Side-by-side
src/kernelPCA.jl
View file @
913bb57e
using
MLKernels
function
kernelPCA
(
X
;
α
=
1.0
)
κ
=
GaussianKernel
(
α
)
K
=
kernelmatrix
(
κ
,
X
)
N
=
size
(
K
)[
1
]
In
=
ones
(
N
,
N
)
./
N
K
=
K
-
In
*
K
-
K
*
In
+
In
*
K
*
In
In
=
fill
(
1
/
N
,
(
N
,
N
)
)
K
=
K
-
In
*
K
-
K
*
In
+
In
*
K
*
In
# Make sure mean is zero
(
D
,
V
)
=
eig
(
K
)
Kpc
=
K
*
V
...
...
src/particle_filters/particle_filter_test.jl
View file @
913bb57e
...
...
@@ -3,12 +3,13 @@ using Devectorize
include
(
"pf_bootstrap.jl"
)
include
(
"pf_smoother_FFBSi.jl"
)
include
(
"pf_PMMH.jl"
)
include
(
"pf_PG.jl"
)
include
(
"resample.jl"
)
include
(
"utilities.jl"
)
const
σw0
=
2.0
const
σw
=
0.32
const
σv
=
1.0
const
theta0
=
[
0.5
,
25
,
8
,
1.2
]
const
theta0
=
[
0.5
,
25
,
8
]
s2piσv
=
log
(
sqrt
(
2
*
pi
)
*
σv
)
function
f
(
x
::
Vector
,
t
::
Int64
)
...
...
@@ -20,6 +21,17 @@ 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
))
function
f_sample
(
x
::
Vector
,
t
::
Int64
)
c
=
8
*
cos
(
1.2
*
(
t
-
1
))
@inbounds
for
i
=
1
:
length
(
x
)
x
[
i
]
=
0.5
*
x
[
i
]
+
25
*
x
[
i
]
./
(
1
+
x
[
i
]
^
2
)
+
c
+
σw
*
randn
()
end
return
x
end
f_sample
(
x
::
Float64
,
t
::
Int64
)
=
0.5
*
x
+
25
*
x
/
(
1
+
x
^
2
)
+
8
*
cos
(
1.2
*
(
t
-
1
))
+
σw
*
randn
()
function
f_density
(
xb
,
xp
,
t
)
x
=
f
(
xp
,
t
)
-
xb
exp
(
-
0.5
*
(
x
/
σw
)
^
2
)
...
...
@@ -35,14 +47,17 @@ end
## Parameter estimation
function
f_theta
(
x
::
Vector
,
t
::
Int64
,
theta
)
c
=
theta
[
3
]
*
cos
(
theta
[
4
]
*
(
t
-
1
))
function
f_theta
_sample
(
x
::
Vector
,
t
,
theta
)
c
=
theta
[
3
]
*
cos
(
1.2
*
(
t
-
1
))
@inbounds
for
i
=
1
:
length
(
x
)
x
[
i
]
=
theta
[
1
]
*
x
[
i
]
+
theta
[
2
]
*
x
[
i
]
./
(
1
+
x
[
i
]
^
2
)
+
c
x
[
i
]
=
theta
[
1
]
*
x
[
i
]
+
theta
[
2
]
*
x
[
i
]
./
(
1
+
x
[
i
]
^
2
)
+
c
+
σw
*
randn
()
end
return
x
end
# f_theta(x::Float64, t::Int64) = theta[1]*x + theta[2]*x/(1+x^2) + theta[3]*cos(1.2*(t-1))
f_theta_sample
(
x
::
Float64
,
t
,
theta
)
=
theta
[
1
]
*
x
+
theta
[
2
]
*
x
./
(
1
+
x
^
2
)
+
theta
[
3
]
*
cos
(
1.2
*
(
t
-
1
))
+
σw
*
randn
()
function
f_density_theta
(
xb
,
xp
,
t
,
theta
)
x
=
f_theta
(
xp
,
t
,
theta
)
-
xb
...
...
@@ -58,10 +73,13 @@ function g_density_theta(y::Float64,x::Vector{Float64},w, theta)
return
w
end
draw_theta
(
theta
)
=
theta
+
0.05
*
randn
(
size
(
theta0
))
.*
abs
(
theta0
)
draw_theta0
()
=
theta0
+
0.1
*
randn
(
size
(
theta0
))
.*
abs
(
theta0
)
draw_theta_random
(
theta
)
=
theta
+
0.01
*
randn
(
size
(
theta0
))
.*
abs
(
theta0
)
draw_theta0
()
=
theta0
+
0.2
*
randn
(
size
(
theta0
))
.*
abs
(
theta0
)
function
draw_theta_PG
(
theta
,
x
)
t
=
0
:
length
(
x
)
-
1
[
x
x
./
(
1
+
x
.^
2
)
cos
(
1.2
*
t
)][
1
:
end
-
1
,
:
]
\
x
[
2
:
end
]
# + draw_theta_random(theta)
end
rms
(
x
)
=
sqrt
(
mean
(
x
.^
2
))
...
...
@@ -86,10 +104,10 @@ function test_pf()
x
[
1
]
=
σw
*
randn
()
y
[
1
]
=
σv
*
randn
()
for
t
=
1
:
T
-
1
x
[
t
+
1
]
=
f
(
x
[
t
],
t
)
+
σw
*
randn
()
x
[
t
+
1
]
=
f
_sample
(
x
[
t
],
t
)
y
[
t
+
1
]
=
0.05
x
[
t
+
1
]
^
2
+
σv
*
randn
()
end
# t
pf_bootstrap!
(
xp
,
w
,
y
,
N
,
g_density
,
f
,
σw0
)
pf_bootstrap!
(
xp
,
w
,
y
,
N
,
g_density
,
f
_sample
,
σw0
)
xb
=
pf_smoother_FFBSi
(
w
,
xp
,
M
,
f_density
)
# weighted_mean!(xh,xp,w)
mode!
(
xh
,
xp
,
w
)
...
...
@@ -112,28 +130,108 @@ using Winston
function
test_PMMH
()
particle_count
=
[
100
]
time_steps
=
[
200
]
R
=
5_000
for
(
Ti
,
T
)
in
enumerate
(
time_steps
)
for
(
Ni
,
N
)
in
enumerate
(
particle_count
)
montecarlo_runs
=
4
# maximum(particle_count)*maximum(time_steps) / T / N*10
x
=
Array
(
Float64
,
T
)
y
=
Array
(
Float64
,
T
)
theta_PMMH
=
SharedArray
(
Float64
,(
size
(
theta0
,
1
),
R
,
montecarlo_runs
))
@sync
@parallel
for
mc_iter
=
1
:
montecarlo_runs
x
[
1
]
=
σw
*
randn
()
y
[
1
]
=
σv
*
randn
()
for
t
=
1
:
T
-
1
x
[
t
+
1
]
=
f_sample
(
x
[
t
],
t
)
y
[
t
+
1
]
=
0.05
x
[
t
+
1
]
^
2
+
σv
*
randn
()
end
# t
theta0i
=
draw_theta0
()
theta_PMMH
[
:
,
:
,
mc_iter
]
=
pf_PMMH
(
y
,
N
,
R
,
pf_bootstrap_nn
,
f_theta_sample
,
draw_theta_random
,
f_density_theta
,
g_density_theta
,
σw0
,
theta0i
)
end
# MC
newplot
(
theta_PMMH
[
:
,
:
,
1
]
'
);
hold
(
true
)
for
mc_iter
=
2
:
montecarlo_runs
plot
(
theta_PMMH
[
:
,
:
,
mc_iter
]
'
)
end
hold
(
false
);
title
(
"PMMH"
)
@show
N
end
# N
@show
T
end
# T
println
(
"Done test_PMMH"
)
# return theta
end
function
test_PG
()
particle_count
=
[
10
]
time_steps
=
[
500
]
R
=
10_000
for
(
Ti
,
T
)
in
enumerate
(
time_steps
)
for
(
Ni
,
N
)
in
enumerate
(
particle_count
)
montecarlo_runs
=
16
# maximum(particle_count)*maximum(time_steps) / T / N*10
montecarlo_runs
=
4
# maximum(particle_count)*maximum(time_steps) / T / N*10
x
=
Array
(
Float64
,
T
)
y
=
Array
(
Float64
,
T
)
theta
=
SharedArray
(
Float64
,(
size
(
theta0
,
1
),
R
,
montecarlo_runs
))
theta
_PG
=
SharedArray
(
Float64
,(
size
(
theta0
,
1
),
R
,
montecarlo_runs
))
@sync
@parallel
for
mc_iter
=
1
:
montecarlo_runs
x
[
1
]
=
σw
*
randn
()
y
[
1
]
=
σv
*
randn
()
for
t
=
1
:
T
-
1
x
[
t
+
1
]
=
f
(
x
[
t
],
t
)
+
σw
*
randn
()
x
[
t
+
1
]
=
f
_sample
(
x
[
t
],
t
)
y
[
t
+
1
]
=
0.05
x
[
t
+
1
]
^
2
+
σv
*
randn
()
end
# t
theta0i
=
draw_theta0
()
theta_PG
[
:
,
:
,
mc_iter
]
=
pf_PG
(
y
,
N
,
R
,
pf_smoother_FFBSi!
,
f_theta_sample
,
draw_theta_PG
,
f_density_theta
,
g_density_theta
,
σw0
,
theta0i
)
end
# MC
newplot
(
theta_PG
[
:
,
:
,
1
]
'
);
hold
(
true
)
for
mc_iter
=
2
:
montecarlo_runs
plot
(
theta_PG
[
:
,
:
,
mc_iter
]
'
)
end
hold
(
false
);
title
(
"PG"
)
th
=
Array
(
theta_PG
)
figure
();
pp
=
plothist
(
th
[
1
,
:
,
:
][
:
],
2000
);
hold
(
true
)
plothist
(
th
[
2
,
:
,
:
][
:
],
2000
)
plothist
(
th
[
3
,
:
,
:
][
:
],
2000
)
hold
(
false
);
display
(
pp
)
return
theta_PG
@show
N
end
# N
@show
T
end
# T
println
(
"Done test_PMMH"
)
# return theta
end
theta
[
:
,
:
,
mc_iter
]
=
pf_PMMH
(
y
,
N
,
R
,
pf_bootstrap_nn
,
f_theta
,
draw_theta
,
f_density_theta
,
g_density_theta
,
σw0
,
draw_theta0
())
function
test_PMMH_PG
()
particle_count
=
[
100
]
time_steps
=
[
200
]
R
=
5_000
for
(
Ti
,
T
)
in
enumerate
(
time_steps
)
for
(
Ni
,
N
)
in
enumerate
(
particle_count
)
montecarlo_runs
=
4
# maximum(particle_count)*maximum(time_steps) / T / N*10
x
=
Array
(
Float64
,
T
)
y
=
Array
(
Float64
,
T
)
theta_PMMH
=
SharedArray
(
Float64
,(
size
(
theta0
,
1
),
R
,
montecarlo_runs
))
theta_PG
=
SharedArray
(
Float64
,(
size
(
theta0
,
1
),
R
,
montecarlo_runs
))
@sync
@parallel
for
mc_iter
=
1
:
montecarlo_runs
x
[
1
]
=
σw
*
randn
()
y
[
1
]
=
σv
*
randn
()
for
t
=
1
:
T
-
1
x
[
t
+
1
]
=
f
(
x
[
t
],
t
)
+
σw
*
randn
()
y
[
t
+
1
]
=
0.05
x
[
t
+
1
]
^
2
+
σv
*
randn
()
end
# t
theta0i
=
draw_theta0
()
theta_PMMH
[
:
,
:
,
mc_iter
]
=
pf_PMMH
(
y
,
N
,
R
,
pf_bootstrap_nn
,
f_theta
,
draw_theta_random
,
f_density_theta
,
g_density_theta
,
σw0
,
theta0i
)
theta_PG
[
:
,
:
,
mc_iter
]
=
pf_PG
(
y
,
N
,
R
,
pf_smoother_FFBSi!
,
f_theta
,
draw_theta_PG
,
f_density_theta
,
g_density_theta
,
σw0
,
theta0i
)
end
# MC
newplot
(
theta
[
:
,
:
,
1
]
'
);
hold
(
true
)
newplot
(
theta
_PMMH
[
:
,
:
,
1
]
'
);
hold
(
true
)
for
mc_iter
=
2
:
montecarlo_runs
plot
(
theta
[
:
,
:
,
mc_iter
]
'
)
plot
(
theta
_PMMH
[
:
,
:
,
mc_iter
]
'
)
end
hold
(
false
)
hold
(
false
);
title
(
"PMMH"
)
newplot
(
theta_PG
[
:
,
:
,
1
]
'
);
hold
(
true
)
for
mc_iter
=
2
:
montecarlo_runs
plot
(
theta_PG
[
:
,
:
,
mc_iter
]
'
)
end
hold
(
false
);
title
(
"PG"
)
@show
N
end
# N
@show
T
...
...
@@ -143,9 +241,7 @@ function test_PMMH()
end
pf_bootstrap!
(
eye
(
4
),
eye
(
4
),
ones
(
4
),
4
,
g_density
,
f
,
σw
)
gc
()
Profile
.
clear
()
function
plotting
(
RMSE
)
...
...
src/particle_filters/pf_PG.jl
0 → 100644
View file @
913bb57e
function
pf_PG
(
y
,
N
,
R
,
pf_smoother
,
f_theta
,
draw_theta
,
f_density_theta
,
g_density_theta
,
sigmaW0
,
theta0
)
n_params
=
length
(
theta0
)
T
=
length
(
y
)
xp
=
Array
(
Float64
,(
N
,
T
))
w
=
Array
(
Float64
,(
N
,
T
))
xb
=
Array
(
Float64
,
1
,
T
)
theta
=
Array
(
Float64
,
n_params
,
R
)
theta
[
:
,
1
]
=
theta0
g_density
(
y_t
,
x_t
,
w
)
=
g_density_theta
(
y_t
,
x_t
,
w
,
theta0
)
f_density
(
xb
,
xp
,
t
)
=
f_density_theta
(
xb
,
xp
,
t
,
theta0
)
f
(
x
,
t
)
=
f_theta
(
x
,
t
,
theta0
)
pf_bootstrap!
(
xp
,
w
,
y
,
N
,
g_density
,
f
)
pf_smoother
(
xb
,
w
,
xp
,
1
,
f_density
)
breakPoints
=
10
# noiseMultiplier = 0.1^(1/R)
Nstart
=
N
breakTime
=
floor
(
Int64
,
R
/
breakPoints
)
particleIncrease
=
10
*
N
thetaP
=
draw_theta
(
theta0
,
xb
'
)
f
(
x
,
t
)
=
f_theta
(
x
,
t
,
thetaP
)
g_density
(
y_t
,
x_t
,
w
)
=
g_density_theta
(
y_t
,
x_t
,
w
,
thetaP
)
f_density
(
xb
,
xp
,
t
)
=
f_density_theta
(
xb
,
xp
,
t
,
thetaP
)
for
r
=
2
:
R
try
pf_CSMC!
(
xp
,
w
,
y
,
N
,
g_density
,
f
,
xb
)
pf_smoother
(
xb
,
w
,
xp
,
1
,
f_density
)
# @show exp(Zp-Z[r-1])
thetaP
=
draw_theta
(
theta
[
:
,
r
-
1
],
xb
'
)
theta
[
:
,
r
]
=
thetaP
catch
ex
display
(
ex
)
@show
r
@show
thetaP
error
(
"Stopping"
)
end
if
(
r
%
breakTime
==
0
)
display
(
r
/
R
)
# N = round(Int64, particleIncrease*(r/R)^3 + Nstart )
# xp = Array(Float64,(N,T))
# w = Array(Float64,(N,T))
# wnn = Array(Float64,(N,T))
end
end
return
theta
end
src/particle_filters/pf_PMMH.jl
View file @
913bb57e
using
Debug
function
pf_PMMH
(
y
,
N
,
R
,
pf
,
f_theta
,
draw_theta
,
f_density_theta
,
g_density_theta
,
sigmaW0
,
theta0
)
function
pf_PMMH
(
y
,
N
,
R
,
pf
,
f_theta
,
draw_theta
,
f_density_theta
,
g_density_theta
,
theta0
)
n_params
=
length
(
theta0
)
T
=
length
(
y
)
...
...
@@ -14,7 +13,7 @@ function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_th
g_density
(
y_t
,
x_t
,
w
)
=
g_density_theta
(
y_t
,
x_t
,
w
,
theta0
)
f
(
x
,
t
)
=
f_theta
(
x
,
t
,
theta0
)
pf
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
,
sigmaW0
)
pf
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
)
Z
[
1
]
=
Zf
(
wnn
)
...
...
@@ -34,7 +33,7 @@ function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_th
# f_density(x_t1, x_t, t) = f_density_theta(x_t1, x_t, t, thetaP)
pf
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
,
sigmaW0
)
pf
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
)
Zp
=
Zf
(
wnn
)
# @show exp(Zp-Z[r-1])
Iaccept
=
rand
()
<
exp
(
Zp
-
Z
[
r
-
1
])
...
...
src/particle_filters/pf_bootstrap.jl
View file @
913bb57e
# module Tmp
using
Devectorize
function
pf_bootstrap!
(
xp
,
w
,
y
,
N
,
g_density
,
f
,
σw
)
function
pf_bootstrap!
(
xp
,
w
,
y
,
N
,
g_density
,
f
)
T
=
length
(
y
)
w0
=
fill
(
log
(
1
/
N
),
N
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
),
1
)
w1
=
copy
(
w0
)
g_density
(
y
[
1
],
xp
[
:
,
1
],
w1
)
w
[
:
,
1
]
=
w1
...
...
@@ -23,7 +23,7 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
+
σw
*
randn
(
N
)
xp
[
:
,
t
]
=
xpT
g_density
(
y
[
t
],
xp
[
:
,
t
],
wT
)
w
[
:
,
t
]
=
wT
...
...
@@ -40,12 +40,12 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
return
nothing
end
function
pf_bootstrap
(
y
,
N
,
g_density
,
f
,
σw
)
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
]
=
2
*
σw
*
randn
(
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
),
1
)
w1
=
copy
(
w0
)
g_density
(
y
[
1
],
xp
[
:
,
1
],
w1
)
w
[
:
,
1
]
=
w1
...
...
@@ -65,7 +65,7 @@ function pf_bootstrap(y, N, g_density, f, σw )
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
+
σw
*
randn
(
N
)
xp
[
:
,
t
]
=
xpT
g_density
(
y
[
t
],
xp
[
:
,
t
],
wT
)
w
[
:
,
t
]
=
wT
...
...
@@ -85,11 +85,11 @@ end
function
pf_bootstrap_nn
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
,
σw
)
function
pf_bootstrap_nn
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
)
T
=
length
(
y
)
w0
=
fill
(
log
(
1
/
N
),
N
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
),
1
)
w1
=
copy
(
w0
)
g_density
(
y
[
1
],
xp
[
:
,
1
],
w1
)
wnn
[
:
,
1
]
=
w1
...
...
@@ -110,7 +110,7 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
+
σw
*
randn
(
N
)
xp
[
:
,
t
]
=
xpT
g_density
(
y
[
t
],
xp
[
:
,
t
],
wT
)
wnn
[
:
,
t
]
=
wT
# Normalize weights
...
...
@@ -130,11 +130,11 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
end
function
pf_aux_nn
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
,
σw
)
function
pf_aux_nn
(
xp
,
w
,
wnn
,
y
,
N
,
g_density
,
f
)
T
=
length
(
y
)
w0
=
fill
(
log
(
1
/
N
),
N
)
xp
[
:
,
1
]
=
2
*
σw
*
randn
(
N
)
xp
[
:
,
1
]
=
f
(
zeros
(
N
),
1
)
wnn
[
:
,
1
]
=
w0
+
g_density
(
y
[
1
],
xp
[
:
,
1
])
@devec
w
[
:
,
1
]
=
wnn
[
:
,
1
]
-
log
(
sum
(
exp
(
wnn
[
:
,
1
])))
j
=
Array
(
Int64
,
N
)
...
...
@@ -164,7 +164,7 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
# Time update
f
(
xpT
,
t
-
1
)
xp
[
:
,
t
]
=
xpT
+
σw
*
randn
(
N
)
xp
[
:
,
t
]
=
xpT
wT
+=
g_density
(
y
[
t
],
xp
[
:
,
t
])
wnn
[
:
,
t
]
=
wT
-
lambdaT
# Normalize weights
...
...
@@ -181,3 +181,46 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
return
xp
,
w
,
wnn
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
[
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
])))
j
=
Array
(
Int64
,
N
)
# tw = Float64(N)
for
t
=
2
:
T
# Resample
if
t
%
2
==
0
resample_systematic!
(
j
,
slice
(
w
,
:
,
t
-
1
),
N
)
xpT
=
xp
[
j
,
t
-
1
]
wT
=
copy
(
w0
)
else
# Resample not needed
xpT
=
xp
[
:
,
t
-
1
]
wT
=
w
[
:
,
t
-
1
]
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
# 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
# tw = 1/sum(w(:,t).*2)
end
return
nothing
end
src/particle_filters/pf_smoother_FFBSi.jl
View file @
913bb57e
...
...
@@ -16,3 +16,21 @@ function pf_smoother_FFBSi( w,xp, M, f_density )
end
return
xb
end
function
pf_smoother_FFBSi!
(
xb
,
w
,
xp
,
M
,
f_density
)
(
N
,
T
)
=
size
(
w
)
wexp
=
exp
(
w
)
j
=
resample_systematic_exp
(
wexp
[
:
,
T
],
M
)
xb
[
:
,
T
]
=
xp
[
j
,
T
]
wb
=
Array
(
Float64
,
N
)
for
t
=
T
-
1
:-
1
:
1
for
m
=
1
:
M
for
n
=
1
:
N
wb
[
n
]
=
wexp
[
n
,
t
]
*
f_density
(
xb
[
m
,
t
+
1
],
xp
[
n
,
t
],
t
)
+
eps
()
end
i
=
draw_one_categorical
(
wb
)
xb
[
m
,
t
]
=
xp
[
i
,
t
]
end
end
return
xb
end
src/particle_filters/resample.jl
View file @
913bb57e
...
...
@@ -76,9 +76,18 @@ end
function
draw_one_categorical
(
w
)
bins
=
cumsum
(
w
)
# devec
s
=
rand
()
*
bins
[
end
]
for
b
=
1
:
length
(
bins
)
if
s
<
bins
[
b
]
return
b
midpoint
=
round
(
Int64
,
length
(
bins
)
/
2
)
if
s
<
bins
[
midpoint
]
for
b
=
1
:
midpoint
if
s
<
bins
[
b
]
return
b
end
end
else
for
b
=
midpoint
:
length
(
bins
)
if
s
<
bins
[
b
]
return
b
end
end
end
end
Write
Preview
Supports
Markdown
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