Skip to content
Snippets Groups Projects
Commit e0ffc590 authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

work on pf

parent 5220be18
Branches
No related tags found
No related merge requests found
using StatsBase, Plots, Distributions
using StatsBase, Plots
function pf!(xp,w, y, N, g, f, σw ) function pf!(xp,w, y, N, g, f, σw )
T = length(y) T = length(y)
...@@ -7,7 +6,7 @@ function pf!(xp,w, y, N, g, f, σw ) ...@@ -7,7 +6,7 @@ function pf!(xp,w, y, N, g, f, σw )
wT = @view w[:,1] wT = @view w[:,1]
xT = @view xp[:,1] xT = @view xp[:,1]
fill!(wT,log(1/N)) fill!(wT,log(1/N))
g(y[1],xT, wT) g(wT, y[1],xT)
wT .-= log(sum(exp,wT)) wT .-= log(sum(exp,wT))
for t = 2:T for t = 2:T
...@@ -25,16 +24,10 @@ function pf!(xp,w, y, N, g, f, σw ) ...@@ -25,16 +24,10 @@ function pf!(xp,w, y, N, g, f, σw )
copy!(wT,wT1) copy!(wT,wT1)
end end
g(y[t],xT, wT) g(wT, y[t],xT)
# Normalize weights logsumexp!(wT)
offset = maximum(wT)
normConstant = 0.0
for i = 1:N
normConstant += exp(w[i,t]-offset)
end
wT .-= log(sum(exp, wT))
end end
xh = zeros(Float64,T) xh = zeros(Float64,T)
...@@ -46,16 +39,26 @@ function pf!(xp,w, y, N, g, f, σw ) ...@@ -46,16 +39,26 @@ function pf!(xp,w, y, N, g, f, σw )
return xh return xh
end 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) function resample(w)
N = length(w) N = length(w)
j = Array(Int64,N) j = Array{Int64}(N)
# bins = cumsum(exp(w)) # devec # bins = cumsum(exp(w)) # devec
bins = Array(Float64,N) bins = Array{Float64}(N)
bins[1] = exp(w[1]) # devec bins[1] = exp(w[1])
for i = 2:N for i = 2:N
bins[i] = bins[i-1] + exp(w[i]) bins[i] = bins[i-1] + exp(w[i])
end end
s = collect((rand()/N+0):1/N:bins[end]) s = (rand()/N+0):1/N:bins[end]
bo = 1 bo = 1
for i = 1:N for i = 1:N
...@@ -90,16 +93,15 @@ f(xn,x,t::Int64) = begin ...@@ -90,16 +93,15 @@ f(xn,x,t::Int64) = begin
end end
f(x::Float64,t::Int64) = 0.5*x + 25*x./(1+x^2) + 8*cos(1.2*(t-1)) 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 pg = Distributions.Normal(0,σv)
const den = 0.5/σv^2 function g(w,y::Float64,x)
function g(y::Float64,x,w)
@inbounds for i = 1:length(w) @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 end
function g(x)
ret = @. -0.5 * (x/σv)^2
end end
rms(x) = sqrt(mean(abs2, x)) rms(x) = sqrt(mean(abs2, x))
function run_test() function run_test()
...@@ -107,14 +109,14 @@ function run_test() ...@@ -107,14 +109,14 @@ function run_test()
time_steps = [20, 50, 100, 200] time_steps = [20, 50, 100, 200]
RMSE = zeros(length(particle_count),length(time_steps)) # Store the RMS errors RMSE = zeros(length(particle_count),length(time_steps)) # Store the RMS errors
propagated_particles = 0 propagated_particles = 0
@progress for (Ti,T) in enumerate(time_steps) for (Ti,T) in enumerate(time_steps)
@progress for (Ni, N) in enumerate(particle_count) for (Ni, N) in enumerate(particle_count)
montecarlo_runs = maximum(particle_count)*maximum(time_steps) / T / N montecarlo_runs = maximum(particle_count)*maximum(time_steps) / T / N
# montecarlo_runs = 1 # montecarlo_runs = 1
xp = Array(Float64,(N,T)) xp = Array{Float64}(N,T)
w = Array(Float64,(N,T)) w = Array{Float64}(N,T)
x = Array(Float64,T) x = Array{Float64}(T)
y = Array(Float64,T) y = Array{Float64}(T)
E = @parallel (+) for mc_iter = 1:montecarlo_runs E = @parallel (+) for mc_iter = 1:montecarlo_runs
x[1] = σw*randn() x[1] = σw*randn()
......
pf_vec.jl 0 → 100644
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)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment