add pf

pf.jl

+
+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)