Huge update to all particle filter files

src/kernelPCA.jl

+using MLKernels
 function kernelPCA(X; α=1.0)
     κ = GaussianKernel(α)
     K = kernelmatrix(κ,X)
     N = size(K)[1]
-    In = ones(N,N)./N
-    K = K-In*K - K*In + In*K*In
+    In = fill(1/N,(N,N))
+    K = K-In*K - K*In + In*K*In # Make sure mean is zero
 
     (D,V) = eig(K)
     Kpc = K*V

src/particle_filters/particle_filter_test.jl

@@ -3,12 +3,13 @@ using Devectorize
 include("pf_bootstrap.jl")
 include("pf_smoother_FFBSi.jl")
 include("pf_PMMH.jl")
+include("pf_PG.jl")
 include("resample.jl")
 include("utilities.jl")
 const σw0 = 2.0
 const σw = 0.32
 const σv = 1.0
-const theta0 = [0.5, 25, 8, 1.2]
+const theta0 = [0.5, 25, 8]
 s2piσv = log(sqrt(2*pi) * σv)
 
 function f(x::Vector, t::Int64)
@@ -20,6 +21,17 @@ 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))
 
+function f_sample(x::Vector, t::Int64)
+    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
+    return x
+end
+f_sample(x::Float64, t::Int64) = 0.5*x + 25*x/(1+x^2) + 8*cos(1.2*(t-1)) + σw*randn()
+
+
+
 function f_density(xb, xp, t)
     x = f(xp,t)-xb
     exp(-0.5 * (x/σw)^2)
@@ -35,14 +47,17 @@ end
 
 
 ## Parameter estimation
-function f_theta(x::Vector, t::Int64, theta)
-    c = theta[3]*cos(theta[4]*(t-1))
+function f_theta_sample(x::Vector, t, theta)
+    c = theta[3]*cos(1.2*(t-1))
     @inbounds for i = 1:length(x)
-        x[i] =  theta[1]*x[i] + theta[2]*x[i]./(1+x[i]^2) + c
+        x[i] =  theta[1]*x[i] + theta[2]*x[i]./(1+x[i]^2) + c + σw*randn()
     end
-
+    return x
 end
-# f_theta(x::Float64, t::Int64) = theta[1]*x + theta[2]*x/(1+x^2) + theta[3]*cos(1.2*(t-1))
+
+f_theta_sample(x::Float64, t, theta) = theta[1]*x + theta[2]*x./(1+x^2) + theta[3]*cos(1.2*(t-1)) + σw*randn()
+
+
 
 function f_density_theta(xb, xp, t, theta)
     x = f_theta(xp,t, theta)-xb
@@ -58,10 +73,13 @@ function g_density_theta(y::Float64,x::Vector{Float64},w, theta)
     return w
 end
 
-draw_theta(theta) = theta + 0.05*randn(size(theta0)).*abs(theta0)
-draw_theta0() = theta0+ 0.1*randn(size(theta0)).*abs(theta0)
-
+draw_theta_random(theta) = theta + 0.01*randn(size(theta0)).*abs(theta0)
+draw_theta0() = theta0+ 0.2*randn(size(theta0)).*abs(theta0)
 
+function draw_theta_PG(theta, x)
+    t = 0:length(x)-1
+    [x x./(1+x.^2) cos(1.2*t)][1:end-1,:]\x[2:end]# + draw_theta_random(theta)
+end
 
 rms(x) = sqrt(mean(x.^2))
 
@@ -86,10 +104,10 @@ function test_pf()
                 x[1] = σw*randn()
                 y[1] = σv*randn()
                 for t = 1:T-1
-                    x[t+1] = f(x[t],t) + σw*randn()
+                    x[t+1] = f_sample(x[t],t)
                     y[t+1] = 0.05x[t+1]^2  + σv*randn()
                 end # t
-                pf_bootstrap!(xp,w, y, N, g_density, f, σw0 )
+                pf_bootstrap!(xp,w, y, N, g_density, f_sample, σw0 )
                 xb = pf_smoother_FFBSi( w,xp, M, f_density )
                 #                 weighted_mean!(xh,xp,w)
                 mode!(xh,xp,w)
@@ -112,28 +130,108 @@ using Winston
 function test_PMMH()
     particle_count = [100]
     time_steps = [200]
+    R = 5_000
+    for (Ti,T) in enumerate(time_steps)
+        for (Ni, N) in enumerate(particle_count)
+            montecarlo_runs = 4# maximum(particle_count)*maximum(time_steps) / T / N*10
+            x = Array(Float64,T)
+            y = Array(Float64,T)
+            theta_PMMH = SharedArray(Float64,(size(theta0,1),R,montecarlo_runs))
+            @sync @parallel for mc_iter = 1:montecarlo_runs
+                x[1] = σw*randn()
+                y[1] = σv*randn()
+                for t = 1:T-1
+                    x[t+1] = f_sample(x[t],t)
+                    y[t+1] = 0.05x[t+1]^2  + σv*randn()
+                end # t
+                theta0i = draw_theta0()
+                theta_PMMH[:,:,mc_iter] = pf_PMMH(y, N, R, pf_bootstrap_nn, f_theta_sample, draw_theta_random, f_density_theta, g_density_theta, σw0, theta0i)
+            end # MC
+            newplot(theta_PMMH[:,:,1]'); hold(true)
+            for mc_iter = 2:montecarlo_runs
+                plot(theta_PMMH[:,:,mc_iter]')
+            end
+            hold(false); title("PMMH")
+            @show N
+        end # N
+        @show T
+    end # T
+    println("Done test_PMMH")
+    #     return theta
+end
+
+function test_PG()
+    particle_count = [10]
+    time_steps = [500]
     R = 10_000
     for (Ti,T) in enumerate(time_steps)
         for (Ni, N) in enumerate(particle_count)
-            montecarlo_runs = 16# maximum(particle_count)*maximum(time_steps) / T / N*10
+            montecarlo_runs = 4# maximum(particle_count)*maximum(time_steps) / T / N*10
             x = Array(Float64,T)
             y = Array(Float64,T)
-            theta = SharedArray(Float64,(size(theta0,1),R,montecarlo_runs))
+            theta_PG = SharedArray(Float64,(size(theta0,1),R,montecarlo_runs))
             @sync @parallel for mc_iter = 1:montecarlo_runs
                 x[1] = σw*randn()
                 y[1] = σv*randn()
                 for t = 1:T-1
-                    x[t+1] = f(x[t],t) + σw*randn()
+                    x[t+1] = f_sample(x[t],t)
                     y[t+1] = 0.05x[t+1]^2  + σv*randn()
                 end # t
+                theta0i = draw_theta0()
+                theta_PG[:,:,mc_iter] = pf_PG(y, N, R, pf_smoother_FFBSi!, f_theta_sample, draw_theta_PG, f_density_theta, g_density_theta, σw0, theta0i )
+            end # MC
+            newplot(theta_PG[:,:,1]'); hold(true)
+            for mc_iter = 2:montecarlo_runs
+                plot(theta_PG[:,:,mc_iter]')
+            end
+            hold(false); title("PG")
+            th = Array(theta_PG)
+            figure(); pp = plothist(th[1,:,:][:],2000); hold(true)
+            plothist(th[2,:,:][:],2000)
+            plothist(th[3,:,:][:],2000)
+            hold(false); display(pp)
+            return theta_PG
+            @show N
+        end # N
+        @show T
+    end # T
+    println("Done test_PMMH")
+    #     return theta
+end
 
-                theta[:,:,mc_iter] = pf_PMMH(y, N, R, pf_bootstrap_nn, f_theta, draw_theta, f_density_theta, g_density_theta, σw0, draw_theta0())
+
+function test_PMMH_PG()
+    particle_count = [100]
+    time_steps = [200]
+    R = 5_000
+    for (Ti,T) in enumerate(time_steps)
+        for (Ni, N) in enumerate(particle_count)
+            montecarlo_runs = 4# maximum(particle_count)*maximum(time_steps) / T / N*10
+            x = Array(Float64,T)
+            y = Array(Float64,T)
+            theta_PMMH = SharedArray(Float64,(size(theta0,1),R,montecarlo_runs))
+            theta_PG = SharedArray(Float64,(size(theta0,1),R,montecarlo_runs))
+            @sync @parallel for mc_iter = 1:montecarlo_runs
+                x[1] = σw*randn()
+                y[1] = σv*randn()
+                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
+                theta0i = draw_theta0()
+                theta_PMMH[:,:,mc_iter] = pf_PMMH(y, N, R, pf_bootstrap_nn, f_theta, draw_theta_random, f_density_theta, g_density_theta, σw0, theta0i)
+                theta_PG[:,:,mc_iter] = pf_PG(y, N, R, pf_smoother_FFBSi!, f_theta, draw_theta_PG, f_density_theta, g_density_theta, σw0, theta0i )
             end # MC
-            newplot(theta[:,:,1]'); hold(true)
+            newplot(theta_PMMH[:,:,1]'); hold(true)
             for mc_iter = 2:montecarlo_runs
-                plot(theta[:,:,mc_iter]')
+                plot(theta_PMMH[:,:,mc_iter]')
             end
-            hold(false)
+            hold(false); title("PMMH")
+            newplot(theta_PG[:,:,1]'); hold(true)
+            for mc_iter = 2:montecarlo_runs
+                plot(theta_PG[:,:,mc_iter]')
+            end
+            hold(false); title("PG")
             @show N
         end # N
         @show T
@@ -143,9 +241,7 @@ function test_PMMH()
 end
 
 
-pf_bootstrap!(eye(4),eye(4), ones(4), 4, g_density, f, σw )
-gc()
-Profile.clear()
+
 
 
 function plotting(RMSE)

src/particle_filters/pf_PG.jl

+
+function pf_PG(y, N, R, pf_smoother, f_theta, draw_theta, f_density_theta, g_density_theta, sigmaW0, theta0 )
+
+    n_params = length(theta0)
+    T = length(y)
+    xp = Array(Float64,(N,T))
+    w = Array(Float64,(N,T))
+    xb = Array(Float64,1,T)
+
+    theta = Array(Float64,n_params,R)
+    theta[:,1] = theta0
+
+    g_density(y_t, x_t,w)  = g_density_theta(y_t, x_t,w, theta0)
+    f_density(xb,xp,t) = f_density_theta(xb, xp, t, theta0)
+    f(x,t) = f_theta(x,t,theta0)
+    pf_bootstrap!(xp, w, y, N, g_density, f)
+    pf_smoother(xb, w,xp, 1, f_density )
+
+    breakPoints = 10
+
+    #     noiseMultiplier = 0.1^(1/R)
+    Nstart = N
+    breakTime = floor(Int64, R / breakPoints)
+    particleIncrease = 10*N
+    thetaP = draw_theta(theta0,xb')
+    f(x,t) = f_theta(x,t,thetaP)
+    g_density(y_t, x_t,w)  = g_density_theta(y_t, x_t,w, thetaP)
+    f_density(xb,xp,t) = f_density_theta(xb, xp, t, thetaP)
+    for r = 2:R
+        try
+            pf_CSMC!(xp, w, y, N, g_density, f, xb)
+            pf_smoother(xb, w,xp, 1, f_density )
+            #         @show exp(Zp-Z[r-1])
+            thetaP = draw_theta(theta[:,r-1],xb')
+            theta[:,r] = thetaP
+        catch ex
+            display(ex)
+            @show r
+            @show thetaP
+            error("Stopping")
+        end
+
+        if (r % breakTime == 0)
+            display(r/R)
+            #             N = round(Int64, particleIncrease*(r/R)^3 + Nstart  )
+            #             xp = Array(Float64,(N,T))
+            #             w = Array(Float64,(N,T))
+            #             wnn = Array(Float64,(N,T))
+        end
+
+
+    end
+
+    return theta
+end
+
+
+
+
+
+

src/particle_filters/pf_PMMH.jl

-using Debug
-function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_theta, sigmaW0, theta0 )
+function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_theta, theta0 )
 
     n_params = length(theta0)
     T = length(y)
@@ -14,7 +13,7 @@ function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_th
 
     g_density(y_t, x_t,w)  = g_density_theta(y_t, x_t,w, theta0)
     f(x,t) = f_theta(x,t,theta0)
-    pf(xp, w, wnn, y, N, g_density, f, sigmaW0)
+    pf(xp, w, wnn, y, N, g_density, f)
 
     Z[1] = Zf(wnn)
 
@@ -34,7 +33,7 @@ function pf_PMMH(y, N, R, pf, f_theta, draw_theta, f_density_theta, g_density_th
         #         f_density(x_t1, x_t, t) = f_density_theta(x_t1, x_t, t, thetaP)
 
 
-        pf(xp, w, wnn, y, N, g_density, f, sigmaW0)
+        pf(xp, w, wnn, y, N, g_density, f)
         Zp = Zf(wnn)
 #         @show exp(Zp-Z[r-1])
         Iaccept = rand() < exp(Zp-Z[r-1])

src/particle_filters/pf_bootstrap.jl

 # module Tmp
 using Devectorize
-function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
+function pf_bootstrap!(xp,w, y, N, g_density, f)
     T = length(y)
     w0 = fill(log(1/N),N)
-    xp[:,1] = 2*σw*randn(N)
+    xp[:,1] = f(zeros(N),1)
     w1 = copy(w0)
     g_density(y[1],xp[:,1], w1)
     w[:,1] = w1
@@ -23,7 +23,7 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
 
         # Time update
         f(xpT,t-1)
-        xp[:,t] = xpT + σw*randn(N)
+        xp[:,t] = xpT
         g_density(y[t],xp[:,t], wT)
         w[:,t] = wT
 
@@ -40,12 +40,12 @@ function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
     return nothing
 end
 
-function pf_bootstrap(y, N, g_density, f, σw )
+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] = 2*σw*randn(N)
+    xp[:,1] = f(zeros(N),1)
     w1 = copy(w0)
     g_density(y[1],xp[:,1], w1)
     w[:,1] = w1
@@ -65,7 +65,7 @@ function pf_bootstrap(y, N, g_density, f, σw )
 
         # Time update
         f(xpT,t-1)
-        xp[:,t] = xpT + σw*randn(N)
+        xp[:,t] = xpT
         g_density(y[t],xp[:,t], wT)
         w[:,t] = wT
 
@@ -85,11 +85,11 @@ end
 
 
 
-function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
+function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f)
     T = length(y)
 
     w0 = fill(log(1/N),N)
-    xp[:,1] = 2*σw*randn(N)
+    xp[:,1] = f(zeros(N),1)
     w1 = copy(w0)
     g_density(y[1],xp[:,1], w1)
     wnn[:,1] = w1
@@ -110,7 +110,7 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
 
         # Time update
         f(xpT,t-1)
-        xp[:,t] = xpT + σw*randn(N)
+        xp[:,t] = xpT
         g_density(y[t],xp[:,t], wT)
         wnn[:,t] = wT
         # Normalize weights
@@ -130,11 +130,11 @@ function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
 end
 
 
-function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
+function pf_aux_nn(xp, w, wnn, y, N, g_density, f)
     T = length(y)
 
     w0 = fill(log(1/N),N)
-    xp[:,1] = 2*σw*randn(N)
+    xp[:,1] = f(zeros(N),1)
     wnn[:,1] = w0 + g_density(y[1],xp[:,1])
     @devec w[:,1] = wnn[:,1] - log(sum(exp(wnn[:,1])))
     j = Array(Int64,N)
@@ -164,7 +164,7 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
 
         # Time update
         f(xpT,t-1)
-        xp[:,t] = xpT + σw*randn(N)
+        xp[:,t] = xpT
         wT += g_density(y[t],xp[:,t])
         wnn[:,t] = wT - lambdaT
         # Normalize weights
@@ -181,3 +181,46 @@ function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
 
     return xp,w, wnn
 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[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])))
+    j = Array(Int64,N)
+    #     tw = Float64(N)
+    for t = 2:T
+        # Resample
+        if t%2 == 0
+            resample_systematic!(j,slice(w,:,t-1),N)
+            xpT = xp[j,t-1]
+            wT = copy(w0)
+        else # Resample not needed
+            xpT = xp[:,t-1]
+            wT = w[:,t-1]
+        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
+
+        # 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
+        #         tw =  1/sum(w(:,t).*2)
+    end
+
+    return nothing
+end

src/particle_filters/pf_smoother_FFBSi.jl

@@ -16,3 +16,21 @@ function pf_smoother_FFBSi( w,xp, M, f_density )
     end
     return xb
 end
+
+function pf_smoother_FFBSi!( xb, w,xp, M, f_density )
+    (N,T) = size(w)
+    wexp = exp(w)
+    j = resample_systematic_exp(wexp[:,T],M)
+    xb[:,T] = xp[j, T]
+    wb = Array(Float64,N)
+    for t = T-1:-1:1
+        for m = 1:M
+            for n = 1:N
+                wb[n] = wexp[n,t]*f_density(xb[m,t+1],xp[n,t],t)+eps()
+            end
+            i = draw_one_categorical(wb)
+            xb[m,t] = xp[i, t]
+        end
+    end
+    return xb
+end

src/particle_filters/resample.jl

@@ -76,9 +76,18 @@ end
 function draw_one_categorical(w)
     bins = cumsum(w) # devec
     s = rand()*bins[end]
-    for b = 1:length(bins)
+    midpoint = round(Int64,length(bins)/2)
+    if s < bins[midpoint]
+        for b = 1:midpoint
             if s < bins[b]
                 return b
             end
         end
+    else
+        for b = midpoint:length(bins)
+            if s < bins[b]
+                return b
+            end
+        end
+    end
 end