From 913bb57e1cfc14867d410d4a08f700b953bc7a49 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <fredrikb@control.lth.se>
Date: Wed, 16 Sep 2015 07:41:52 +0200
Subject: [PATCH] Huge update to all particle filter files
---
src/kernelPCA.jl | 5 +-
src/particle_filters/particle_filter_test.jl | 138 ++++++++++++++++---
src/particle_filters/pf_PG.jl | 61 ++++++++
src/particle_filters/pf_PMMH.jl | 7 +-
src/particle_filters/pf_bootstrap.jl | 67 +++++++--
src/particle_filters/pf_smoother_FFBSi.jl | 18 +++
src/particle_filters/resample.jl | 15 +-
7 files changed, 269 insertions(+), 42 deletions(-)
create mode 100644 src/particle_filters/pf_PG.jl
diff --git a/src/kernelPCA.jl b/src/kernelPCA.jl
index 484f7a3..f6d727e 100644
--- a/src/kernelPCA.jl
+++ b/src/kernelPCA.jl
@@ -1,9 +1,10 @@
+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
diff --git a/src/particle_filters/particle_filter_test.jl b/src/particle_filters/particle_filter_test.jl
index e1294d0..c13f89b 100644
--- a/src/particle_filters/particle_filter_test.jl
+++ b/src/particle_filters/particle_filter_test.jl
@@ -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.05x[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.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
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.05x[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.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
- 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)
diff --git a/src/particle_filters/pf_PG.jl b/src/particle_filters/pf_PG.jl
new file mode 100644
index 0000000..e721e63
--- /dev/null
+++ b/src/particle_filters/pf_PG.jl
@@ -0,0 +1,61 @@
+
+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
+
+
+
+
+
+
diff --git a/src/particle_filters/pf_PMMH.jl b/src/particle_filters/pf_PMMH.jl
index b87b5f0..6f3dd0e 100644
--- a/src/particle_filters/pf_PMMH.jl
+++ b/src/particle_filters/pf_PMMH.jl
@@ -1,5 +1,4 @@
-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])
diff --git a/src/particle_filters/pf_bootstrap.jl b/src/particle_filters/pf_bootstrap.jl
index 35f2b60..e7ab1e5 100644
--- a/src/particle_filters/pf_bootstrap.jl
+++ b/src/particle_filters/pf_bootstrap.jl
@@ -1,9 +1,9 @@
# 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
diff --git a/src/particle_filters/pf_smoother_FFBSi.jl b/src/particle_filters/pf_smoother_FFBSi.jl
index 2f725a8..ee341cd 100644
--- a/src/particle_filters/pf_smoother_FFBSi.jl
+++ b/src/particle_filters/pf_smoother_FFBSi.jl
@@ -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
diff --git a/src/particle_filters/resample.jl b/src/particle_filters/resample.jl
index 293b90c..795f663 100644
--- a/src/particle_filters/resample.jl
+++ b/src/particle_filters/resample.jl
@@ -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
--
GitLab