From cbb84401a42f85aa091110af50ec451f1313bb21 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlsson <cont-frb@ulund.org>
Date: Sat, 19 Sep 2015 12:19:37 +0200
Subject: [PATCH] updates to particle filters, speedy performance improvements
---
src/particle_filters/particle_filter_test.jl | 7 +
src/particle_filters/pf_bootstrap.jl | 163 +++++++++----------
2 files changed, 80 insertions(+), 90 deletions(-)
diff --git a/src/particle_filters/particle_filter_test.jl b/src/particle_filters/particle_filter_test.jl
index c13f89b..3698ee7 100644
--- a/src/particle_filters/particle_filter_test.jl
+++ b/src/particle_filters/particle_filter_test.jl
@@ -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)
diff --git a/src/particle_filters/pf_bootstrap.jl b/src/particle_filters/pf_bootstrap.jl
index e7ab1e5..5cdb02c 100644
--- a/src/particle_filters/pf_bootstrap.jl
+++ b/src/particle_filters/pf_bootstrap.jl
@@ -1,39 +1,36 @@
# 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(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
- 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(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
- 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)
-
- w0 = fill(log(1/N),N)
- xp[:,1] = f(zeros(N),1)
- w1 = copy(w0)
- g_density(y[1],xp[:,1], w1)
- wnn[:,1] = w1
- @devec w[:,1] = wnn[:,1] - log(sum(exp(wnn[:,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)
+ wnn[:,t] = 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,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!(wT,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
--
GitLab