Skip to content

GitLab

Commit cbb84401 authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

updates to particle filters, speedy performance improvements

parent 913bb57e
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(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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment