From 6695ecac6dfa2cd8e7e7c215f4867aa337e26381 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <fredrikb@control.lth.se>
Date: Sun, 13 Sep 2015 23:46:35 +0200
Subject: [PATCH] added pf_bootstrap

---
 src/particle_filters/pf_bootstrap.jl | 183 +++++++++++++++++++++++++++
 1 file changed, 183 insertions(+)
 create mode 100644 src/particle_filters/pf_bootstrap.jl

diff --git a/src/particle_filters/pf_bootstrap.jl b/src/particle_filters/pf_bootstrap.jl
new file mode 100644
index 0000000..35f2b60
--- /dev/null
+++ b/src/particle_filters/pf_bootstrap.jl
@@ -0,0 +1,183 @@
+# 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
-- 
GitLab