Commit 913bb57e authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

Huge update to all particle filter files

parent 6695ecac
using MLKernels
function kernelPCA(X; α=1.0) function kernelPCA(X; α=1.0)
κ = GaussianKernel(α) κ = GaussianKernel(α)
K = kernelmatrix(κ,X) K = kernelmatrix(κ,X)
N = size(K)[1] N = size(K)[1]
In = ones(N,N)./N In = fill(1/N,(N,N))
K = K-In*K - K*In + In*K*In K = K-In*K - K*In + In*K*In # Make sure mean is zero
(D,V) = eig(K) (D,V) = eig(K)
Kpc = K*V Kpc = K*V
......
...@@ -3,12 +3,13 @@ using Devectorize ...@@ -3,12 +3,13 @@ using Devectorize
include("pf_bootstrap.jl") include("pf_bootstrap.jl")
include("pf_smoother_FFBSi.jl") include("pf_smoother_FFBSi.jl")
include("pf_PMMH.jl") include("pf_PMMH.jl")
include("pf_PG.jl")
include("resample.jl") include("resample.jl")
include("utilities.jl") include("utilities.jl")
const σw0 = 2.0 const σw0 = 2.0
const σw = 0.32 const σw = 0.32
const σv = 1.0 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) s2piσv = log(sqrt(2*pi) * σv)
function f(x::Vector, t::Int64) function f(x::Vector, t::Int64)
...@@ -20,6 +21,17 @@ function f(x::Vector, t::Int64) ...@@ -20,6 +21,17 @@ function f(x::Vector, t::Int64)
end end
f(x::Float64, t::Int64) = 0.5*x + 25*x/(1+x^2) + 8*cos(1.2*(t-1)) 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) function f_density(xb, xp, t)
x = f(xp,t)-xb x = f(xp,t)-xb
exp(-0.5 * (x/σw)^2) exp(-0.5 * (x/σw)^2)
...@@ -35,14 +47,17 @@ end ...@@ -35,14 +47,17 @@ end
## Parameter estimation ## Parameter estimation
function f_theta(x::Vector, t::Int64, theta) function f_theta_sample(x::Vector, t, theta)
c = theta[3]*cos(theta[4]*(t-1)) c = theta[3]*cos(1.2*(t-1))
@inbounds for i = 1:length(x) @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 end
return x
end 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) function f_density_theta(xb, xp, t, theta)
x = f_theta(xp,t, theta)-xb x = f_theta(xp,t, theta)-xb
...@@ -58,10 +73,13 @@ function g_density_theta(y::Float64,x::Vector{Float64},w, theta) ...@@ -58,10 +73,13 @@ function g_density_theta(y::Float64,x::Vector{Float64},w, theta)
return w return w
end end
draw_theta(theta) = theta + 0.05*randn(size(theta0)).*abs(theta0) draw_theta_random(theta) = theta + 0.01*randn(size(theta0)).*abs(theta0)
draw_theta0() = theta0+ 0.1*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)) rms(x) = sqrt(mean(x.^2))
...@@ -86,10 +104,10 @@ function test_pf() ...@@ -86,10 +104,10 @@ function test_pf()
x[1] = σw*randn() x[1] = σw*randn()
y[1] = σv*randn() y[1] = σv*randn()
for t = 1:T-1 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.05x[t+1]^2 + σv*randn() y[t+1] = 0.05x[t+1]^2 + σv*randn()
end # t 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 ) xb = pf_smoother_FFBSi( w,xp, M, f_density )
# weighted_mean!(xh,xp,w) # weighted_mean!(xh,xp,w)
mode!(xh,xp,w) mode!(xh,xp,w)
...@@ -112,28 +130,108 @@ using Winston ...@@ -112,28 +130,108 @@ using Winston
function test_PMMH() function test_PMMH()
particle_count = [100] particle_count = [100]
time_steps = [200] 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.05x[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 R = 10_000
for (Ti,T) in enumerate(time_steps) for (Ti,T) in enumerate(time_steps)
for (Ni, N) in enumerate(particle_count) 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) x = Array(Float64,T)
y = 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 @sync @parallel for mc_iter = 1:montecarlo_runs
x[1] = σw*randn() x[1] = σw*randn()
y[1] = σv*randn() y[1] = σv*randn()
for t = 1:T-1 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.05x[t+1]^2 + σv*randn() y[t+1] = 0.05x[t+1]^2 + σv*randn()
end # t 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.05x[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 end # MC
newplot(theta[:,:,1]'); hold(true) newplot(theta_PMMH[:,:,1]'); hold(true)
for mc_iter = 2:montecarlo_runs for mc_iter = 2:montecarlo_runs
plot(theta[:,:,mc_iter]') plot(theta_PMMH[:,:,mc_iter]')
end 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 @show N
end # N end # N
@show T @show T
...@@ -143,9 +241,7 @@ function test_PMMH() ...@@ -143,9 +241,7 @@ function test_PMMH()
end end
pf_bootstrap!(eye(4),eye(4), ones(4), 4, g_density, f, σw )
gc()
Profile.clear()
function plotting(RMSE) function plotting(RMSE)
......
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
using Debug function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_theta, theta0 )
function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_theta, sigmaW0, theta0 )
n_params = length(theta0) n_params = length(theta0)
T = length(y) T = length(y)
...@@ -14,7 +13,7 @@ function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_th ...@@ -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) g_density(y_t, x_t,w) = g_density_theta(y_t, x_t,w, theta0)
f(x,t) = f_theta(x,t,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) 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 ...@@ -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) # 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) Zp = Zf(wnn)
# @show exp(Zp-Z[r-1]) # @show exp(Zp-Z[r-1])
Iaccept = rand() < exp(Zp-Z[r-1]) Iaccept = rand() < exp(Zp-Z[r-1])
......
# module Tmp # module Tmp
using Devectorize 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) T = length(y)
w0 = fill(log(1/N),N) w0 = fill(log(1/N),N)
xp[:,1] = 2*σw*randn(N) xp[:,1] = f(zeros(N),1)
w1 = copy(w0) w1 = copy(w0)
g_density(y[1],xp[:,1], w1) g_density(y[1],xp[:,1], w1)
w[:,1] = w1 w[:,1] = w1
...@@ -23,7 +23,7 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw ) ...@@ -23,7 +23,7 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
# Time update # Time update
f(xpT,t-1) f(xpT,t-1)
xp[:,t] = xpT + σw*randn(N) xp[:,t] = xpT
g_density(y[t],xp[:,t], wT) g_density(y[t],xp[:,t], wT)
w[:,t] = wT w[:,t] = wT
...@@ -40,12 +40,12 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw ) ...@@ -40,12 +40,12 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
return nothing return nothing
end end
function pf_bootstrap(y, N, g_density, f, σw ) function pf_bootstrap(y, N, g_density, f)
T = length(y) T = length(y)
xp = Array(Float64,(N,T)) xp = Array(Float64,(N,T))
w = Array(Float64,(N,T)) w = Array(Float64,(N,T))
w0 = fill(log(1/N),N) w0 = fill(log(1/N),N)
xp[:,1] = 2*σw*randn(N) xp[:,1] = f(zeros(N),1)
w1 = copy(w0) w1 = copy(w0)
g_density(y[1],xp[:,1], w1) g_density(y[1],xp[:,1], w1)
w[:,1] = w1 w[:,1] = w1
...@@ -65,7 +65,7 @@ function pf_bootstrap(y, N, g_density, f, σw ) ...@@ -65,7 +65,7 @@ function pf_bootstrap(y, N, g_density, f, σw )
# Time update # Time update
f(xpT,t-1) f(xpT,t-1)
xp[:,t] = xpT + σw*randn(N) xp[:,t] = xpT
g_density(y[t],xp[:,t], wT) g_density(y[t],xp[:,t], wT)
w[:,t] = wT w[:,t] = wT
...@@ -85,11 +85,11 @@ end ...@@ -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) T = length(y)
w0 = fill(log(1/N),N) w0 = fill(log(1/N),N)
xp[:,1] = 2*σw*randn(N) xp[:,1] = f(zeros(N),1)
w1 = copy(w0) w1 = copy(w0)
g_density(y[1],xp[:,1], w1) g_density(y[1],xp[:,1], w1)
wnn[:,1] = w1 wnn[:,1] = w1
...@@ -110,7 +110,7 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw ) ...@@ -110,7 +110,7 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
# Time update # Time update
f(xpT,t-1) f(xpT,t-1)
xp[:,t] = xpT + σw*randn(N) xp[:,t] = xpT
g_density(y[t],xp[:,t], wT) g_density(y[t],xp[:,t], wT)
wnn[:,t] = wT wnn[:,t] = wT
# Normalize weights # Normalize weights
...@@ -130,11 +130,11 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw ) ...@@ -130,11 +130,11 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
end 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) T = length(y)
w0 = fill(log(1/N),N) 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]) wnn[:,1] = w0 + g_density(y[1],xp[:,1])
@devec w[:,1] = wnn[:,1] - log(sum(exp(wnn[:,1]))) @devec w[:,1] = wnn[:,1] - log(sum(exp(wnn[:,1])))
j = Array(Int64,N) j = Array(Int64,N)
...@@ -164,7 +164,7 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw ) ...@@ -164,7 +164,7 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
# Time update # Time update
f(xpT,t-1) f(xpT,t-1)
xp[:,t] = xpT + σw*randn(N) xp[:,t] = xpT
wT += g_density(y[t],xp[:,t]) wT += g_density(y[t],xp[:,t])
wnn[:,t] = wT - lambdaT wnn[:,t] = wT - lambdaT
# Normalize weights # Normalize weights
...@@ -181,3 +181,46 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw ) ...@@ -181,3 +181,46 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
return xp,w, wnn return xp,w, wnn
end 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
...@@ -16,3 +16,21 @@ function pf_smoother_FFBSi( w,xp, M, f_density ) ...@@ -16,3 +16,21 @@ function pf_smoother_FFBSi( w,xp, M, f_density )
end end
return xb return xb
end 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)