From 8575ae16277de16f2498edea9a15b5858bbf225f Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <cont-frb@ulund.org>
Date: Mon, 12 Feb 2018 18:53:53 +0100
Subject: [PATCH] add pf
---
pf.jl | 158 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 158 insertions(+)
create mode 100644 pf.jl
diff --git a/pf.jl b/pf.jl
new file mode 100644
index 0000000..d7bf98a
--- /dev/null
+++ b/pf.jl
@@ -0,0 +1,158 @@
+
+using StatsBase, Plots
+
+function pf!(xp,w, y, N, g, f, σw )
+ T = length(y)
+ xp[:,1] = 2σw*randn(N)
+ wT = @view w[:,1]
+ xT = @view xp[:,1]
+ fill!(wT,log(1/N))
+ g(y[1],xT, wT)
+ wT .-= log(sum(exp, wT))
+
+ for t = 2:T
+ # Resample
+ xT1 = xT
+ xT = @view xp[:,t]
+ wT1 = wT
+ wT = @view w[:,t]
+ if t % 21 == 0
+ j = resample(wT1)
+ f(xT,xT1[j],t-1)
+ fill!(wT,log(1/N))
+ else # Resample not needed
+ f(xT,xT1,t-1)
+ copy!(wT,wT1)
+ end
+
+ g(y[t],xT, wT)
+
+
+ # Normalize weights
+ offset = maximum(wT)
+ normConstant = 0.0
+ for i = 1:N
+ normConstant += exp(w[i,t]-offset)
+ end
+ wT .-= log(sum(exp, wT))
+
+ end
+ xh = zeros(Float64,T)
+ for j = 1:T
+ @inbounds @simd for i = 1:N
+ xh[j] += xp[i,j]*exp(w[i,j])
+ end
+ end
+ return xh
+end
+
+function resample(w)
+ N = length(w)
+ j = Array(Int64,N)
+# bins = cumsum(exp(w)) # devec
+ bins = Array(Float64,N)
+ bins[1] = exp(w[1]) # devec
+ for i = 2:N
+ bins[i] = bins[i-1] + exp(w[i])
+ end
+ s = collect((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 σw0 = 2.0
+const σw = 1.0
+const σv = 1.0
+
+
+# f(x,t::Int64) = begin
+# 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
+# end
+
+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
+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)
+ @inbounds for i = 1:length(w)
+ w[i] -= den * (y-0.05x[i]^2)^2
+ end
+end
+function g(x)
+ ret = @. -0.5 * (x/σv)^2
+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
+ @progress for (Ti,T) in enumerate(time_steps)
+ @progress 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],t) + σw*randn()
+ y[t+1] = 0.05x[t+1]^2 + σv*randn()
+ end # t
+ xh = pf!(xp,w, y, N, g, f, σw0 )
+ 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