From e0ffc5902acbec8bbe502532d6ef174ca2614a64 Mon Sep 17 00:00:00 2001
From: baggepinnen <cont-frb@ulund.org>
Date: Wed, 14 Feb 2018 06:17:05 +0100
Subject: [PATCH] work on pf
---
pf.jl | 60 +++++++++++-----------
pf_vec.jl | 149 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 files changed, 180 insertions(+), 29 deletions(-)
create mode 100644 pf_vec.jl
diff --git a/pf.jl b/pf.jl
index d7bf98a..bc89631 100644
--- a/pf.jl
+++ b/pf.jl
@@ -1,5 +1,4 @@
-
-using StatsBase, Plots
+using StatsBase, Plots, Distributions
function pf!(xp,w, y, N, g, f, σw )
T = length(y)
@@ -7,8 +6,8 @@ function pf!(xp,w, y, N, g, f, σw )
wT = @view w[:,1]
xT = @view xp[:,1]
fill!(wT,log(1/N))
- g(y[1],xT, wT)
- wT .-= log(sum(exp, wT))
+ g(wT, y[1],xT)
+ wT .-= log(sum(exp,wT))
for t = 2:T
# Resample
@@ -25,16 +24,10 @@ function pf!(xp,w, y, N, g, f, σw )
copy!(wT,wT1)
end
- g(y[t],xT, wT)
+ g(wT, y[t],xT)
- # Normalize weights
- offset = maximum(wT)
- normConstant = 0.0
- for i = 1:N
- normConstant += exp(w[i,t]-offset)
- end
- wT .-= log(sum(exp, wT))
+ logsumexp!(wT)
end
xh = zeros(Float64,T)
@@ -46,16 +39,26 @@ function pf!(xp,w, y, N, g, f, σw )
return xh
end
+@inline logsumexp!(w) = w .-= log(sum(exp, w))
+# function logsumexp!(w)
+# offset = maximum(wT)
+# normConstant = 0.0
+# for i = 1:N
+# normConstant += exp(w[i,t]-offset)
+# end
+# wT .-= log(normConstant) + offset
+# end
+
function resample(w)
N = length(w)
- j = Array(Int64,N)
+ j = Array{Int64}(N)
# bins = cumsum(exp(w)) # devec
- bins = Array(Float64,N)
- bins[1] = exp(w[1]) # devec
+ bins = Array{Float64}(N)
+ bins[1] = exp(w[1])
for i = 2:N
bins[i] = bins[i-1] + exp(w[i])
end
- s = collect((rand()/N+0):1/N:bins[end])
+ s = (rand()/N+0):1/N:bins[end]
bo = 1
for i = 1:N
@@ -90,16 +93,15 @@ f(xn,x,t::Int64) = begin
end
f(x::Float64,t::Int64) = 0.5*x + 25*x./(1+x^2) + 8*cos(1.2*(t-1))
-# g(y[t]-0.05xp[:,t].^2, wT)
-const den = 0.5/σv^2
-function g(y::Float64,x,w)
+const pg = Distributions.Normal(0,σv)
+function g(w,y::Float64,x)
@inbounds for i = 1:length(w)
- w[i] -= den * (y-0.05x[i]^2)^2
+ w[i] -= logpdf(pg, y-0.05x[i]^2)
end
end
-function g(x)
- ret = @. -0.5 * (x/σv)^2
-end
+
+
+
rms(x) = sqrt(mean(abs2, x))
function run_test()
@@ -107,14 +109,14 @@ function run_test()
time_steps = [20, 50, 100, 200]
RMSE = zeros(length(particle_count),length(time_steps)) # Store the RMS errors
propagated_particles = 0
- @progress for (Ti,T) in enumerate(time_steps)
- @progress for (Ni, N) in enumerate(particle_count)
+ for (Ti,T) in enumerate(time_steps)
+ for (Ni, N) in enumerate(particle_count)
montecarlo_runs = maximum(particle_count)*maximum(time_steps) / T / N
# montecarlo_runs = 1
- xp = Array(Float64,(N,T))
- w = Array(Float64,(N,T))
- x = Array(Float64,T)
- y = Array(Float64,T)
+ xp = Array{Float64}(N,T)
+ w = Array{Float64}(N,T)
+ x = Array{Float64}(T)
+ y = Array{Float64}(T)
E = @parallel (+) for mc_iter = 1:montecarlo_runs
x[1] = σw*randn()
diff --git a/pf_vec.jl b/pf_vec.jl
new file mode 100644
index 0000000..91b47f9
--- /dev/null
+++ b/pf_vec.jl
@@ -0,0 +1,149 @@
+using StatsBase, Plots, Distributions, StaticArrays
+
+function init_pf(N, p0)
+ xprev = rand(p0,N)
+ x = similar(x)
+ w = fill(log(1/N), N)
+ x,xprev,w
+end
+
+function pf!(x, xprev, w, u, y, g, f)
+ N = length(x)
+ if shouldresample(w)
+ j = resample(w)
+ f(x,xprev,u,j)
+ fill!(w,log(1/N))
+ else # Resample not needed
+ f(x,xprev,u,1:N)
+ end
+ g(w,y,x)
+ logsumexp!(w)
+ x
+end
+
+function weigthed_mean(x,w)
+ xh = zeros(size(x[1]))
+ @inbounds @simd for i = eachindex(x)
+ xh .+= x[i].*exp(w[i])
+ end
+ return xh
+end
+
+@inline logsumexp!(w) = w .-= log(sum(exp, w))
+# function logsumexp!(w)
+# offset = maximum(wT)
+# normConstant = 0.0
+# for i = 1:N
+# normConstant += exp(w[i,t]-offset)
+# end
+# wT .-= log(normConstant) + offset
+# end
+
+function resample(w)
+ N = length(w)
+ j = Array{Int64}(N)
+ bins = Array{Float64}(N)
+ bins[1] = exp(w[1])
+ for i = 2:N
+ bins[i] = bins[i-1] + exp(w[i])
+ end
+ s = (rand()/N+0):1/N:bins[end]
+
+ bo = 1
+ for i = 1:N
+ @inbounds for b = bo:N
+ if s[i] < bins[b]
+ j[i] = b
+ bo = b
+ break
+ end
+ end
+ end
+ return j
+end
+
+const pg = Distributions.Normal(0,1.0)
+const pf = Distributions.Normal(0,1.0)
+const p0 = Distributions.Normal(0,2.0)
+
+n = 7
+m = 2
+p = 2
+T = randn(n,n)
+const A = SMatrix{n,n}(T*diagm(linspace(0.5,0.99,n))/T)
+const B = @SMatrix randn(n,m)
+const C = @SMatrix randn(p,n)
+
+function f(x,xp,u,j)
+ @inbounds for i = eachindex(x)
+ x[i] = A*xp[j[i]] + B*u + rand(pf)
+ end
+end
+function f(x,u)
+ x[i] = A*xp[j[i]] + B*u + rand(pf)
+end
+
+function g(w,y::Float64,x)
+ @inbounds for i = 1:length(w)
+ w[i] -= logpdf(pg, y-C*x[i])
+ end
+end
+
+
+
+rms(x) = sqrt(mean(abs2, x))
+
+function run_test()
+ particle_count = [5 10 20 50 100 200 500 1000 10_000]
+ time_steps = [20, 50, 100, 200]
+ RMSE = zeros(length(particle_count),length(time_steps)) # Store the RMS errors
+ propagated_particles = 0
+ for (Ti,T) in enumerate(time_steps)
+ for (Ni, N) in enumerate(particle_count)
+ montecarlo_runs = maximum(particle_count)*maximum(time_steps) / T / N
+ # montecarlo_runs = 1
+ xp = Array{Float64}(N,T)
+ w = Array{Float64}(N,T)
+ x = Array{Float64}(T)
+ y = Array{Float64}(T)
+
+ E = @parallel (+) for mc_iter = 1:montecarlo_runs
+ x[1] = σw*randn()
+ y[1] = σv*randn()
+ @inbounds for t = 1:T-1
+ x[t+1] = f(x[t],u) + σw*randn()
+ y[t+1] = 0.05x[t+1]^2 + σv*randn()
+ end # t
+ xh = pf!(x, xprev, w, u, y, g, f)
+ rms(x-xh)
+ end # MC
+ RMSE[Ni,Ti] = E/montecarlo_runs
+ propagated_particles += montecarlo_runs*N*T
+ # figure();plot([x xh])
+
+ @show N
+ end # N
+ @show T
+end # T
+println("Propagated $propagated_particles particles")
+#
+return RMSE
+end
+
+@time pf!(eye(4),eye(4), ones(4), 4, g, f, σw )
+gc()
+Profile.clear()
+
+@time RMSE = run_test()
+
+# Profile.print()
+function plotting(RMSE)
+ time_steps = [20, 50, 100, 200]
+ particle_count = [5, 10, 20, 50, 100, 200, 500, 1000, 10_000]
+ nT = length(time_steps)
+ leg = reshape(["$(time_steps[i]) time steps" for i = 1:nT], 1,:)
+
+ plot(particle_count,RMSE,xscale=:log10, ylabel="RMS errors", xlabel=" Number of particles", lab=leg)
+end
+
+plotting(RMSE)
--
GitLab