added pf_bootstrap

src/particle_filters/pf_bootstrap.jl

+# module Tmp
+using Devectorize
+function pf_bootstrap!(xp,w, y, N, g_density, f, σw )
+    T = length(y)
+    w0 = fill(log(1/N),N)
+    xp[:,1] = 2*σw*randn(N)
+    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 + σw*randn(N)
+        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
+
+function pf_bootstrap(y, N, g_density, f, σw )
+    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)
+    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 + σw*randn(N)
+        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 xp,w
+end
+
+
+
+
+function pf_bootstrap_nn(xp, w, wnn, y, N, g_density, f, σw )
+    T = length(y)
+
+    w0 = fill(log(1/N),N)
+    xp[:,1] = 2*σw*randn(N)
+    w1 = copy(w0)
+    g_density(y[1],xp[:,1], w1)
+    wnn[:,1] = w1
+    @devec w[:,1] = wnn[:,1] - log(sum(exp(wnn[:,1])))
+    j = Array(Int64,N)
+    #     tw = Float64(N)
+    for t = 2:T
+        # Resample
+        if t%2 == 0
+            resample_systematic!(j,w[:,t-1],N)
+            #             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 = copy(w[:,t-1])
+        end
+
+        # Time update
+        f(xpT,t-1)
+        xp[:,t] = xpT + σw*randn(N)
+        g_density(y[t],xp[:,t], wT)
+        wnn[:,t] = wT
+        # Normalize weights
+        #some strange error with this normalization method, I totally have no clue why it's working elsewhere but not here
+        offset = maximum(wnn[:,t])
+        normConstant = 0.0
+        for i = 1:N
+            normConstant += exp(wnn[i,t]-offset)
+        end
+        nc = log(normConstant)+offset
+        w[:,t] = wnn[:,t]- nc
+#         @assert isapprox(nc,log(sum(exp(wnn[:,t]))))
+        #         tw =  1/sum(w(:,t).*2)
+    end
+
+    return xp,w, wnn
+end
+
+
+function pf_aux_nn(xp, w, wnn, y, N, g_density, f, σw )
+    T = length(y)
+
+    w0 = fill(log(1/N),N)
+    xp[:,1] = 2*σw*randn(N)
+    wnn[:,1] = w0 + g_density(y[1],xp[:,1])
+    @devec w[:,1] = wnn[:,1] - log(sum(exp(wnn[:,1])))
+    j = Array(Int64,N)
+    #     tw = Float64(N)
+    for t = 2:T
+
+        xpT = xp[:,t-1]
+        f(xpT,t-1)
+
+        wT = copy(w[:,t-1])
+        lambda = g_density(y[t],xpT)
+        wT += lambda
+        wTe = exp(wT)
+        wTe /= sum(wTe)
+
+        # Resample
+        if t%2 == 0
+            resample_systematic_exp!(j,wTe)
+            xpT = xp[j,t-1]
+            wT = copy(w0)
+            lambdaT = lambda[j]
+        else # Resample not needed
+            xpT = xp[:,t-1]
+            #             wT = copy(w[:,t-1])
+            lambdaT = lambda
+        end
+
+        # Time update
+        f(xpT,t-1)
+        xp[:,t] = xpT + σw*randn(N)
+        wT += g_density(y[t],xp[:,t])
+        wnn[:,t] = wT - lambdaT
+        # Normalize weights
+        #some strange error with this normalization method, I totally have no clue why it's working elsewhere but not here
+        # #         offset = maximum(wT)
+        # #         normConstant = 0.0
+        # #         for i = 1:N
+        # #             normConstant += exp(wT[i]-offset)
+        # #         end
+        # #         w[:,t] = wT - log(normConstant)+offset
+        w[:,t] = wnn[:,t] - log(sum(exp(wnn[:,t])))
+        #         tw =  1/sum(w(:,t).*2)
+    end
+
+    return xp,w, wnn
+end