From 826bede66dad367c5a31fc706c2ca9eb52f094d2 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <fredrikb@control.lth.se>
Date: Sun, 13 Sep 2015 09:45:17 +0200
Subject: [PATCH] Added some particle filter algorithms, not the optimized
 filter though

---
 .../particle_smoother_RS-FFBSi.jl             | 18 +++++
 src/particle_filters/pf_smoother_FFBSi.jl     | 18 +++++
 src/particle_filters/resample.jl              | 66 +++++++++++++++++++
 src/particle_filters/utilities.jl             | 40 +++++++++++
 4 files changed, 142 insertions(+)
 create mode 100644 src/particle_filters/particle_smoother_RS-FFBSi.jl
 create mode 100644 src/particle_filters/pf_smoother_FFBSi.jl
 create mode 100644 src/particle_filters/resample.jl
 create mode 100644 src/particle_filters/utilities.jl

diff --git a/src/particle_filters/particle_smoother_RS-FFBSi.jl b/src/particle_filters/particle_smoother_RS-FFBSi.jl
new file mode 100644
index 0000000..2f725a8
--- /dev/null
+++ b/src/particle_filters/particle_smoother_RS-FFBSi.jl
@@ -0,0 +1,18 @@
+function pf_smoother_FFBSi( w,xp, M, f_density )
+    (N,T) = size(w)
+    xb = Array(Float64,M,T)
+    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
diff --git a/src/particle_filters/pf_smoother_FFBSi.jl b/src/particle_filters/pf_smoother_FFBSi.jl
new file mode 100644
index 0000000..2f725a8
--- /dev/null
+++ b/src/particle_filters/pf_smoother_FFBSi.jl
@@ -0,0 +1,18 @@
+function pf_smoother_FFBSi( w,xp, M, f_density )
+    (N,T) = size(w)
+    xb = Array(Float64,M,T)
+    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
diff --git a/src/particle_filters/resample.jl b/src/particle_filters/resample.jl
new file mode 100644
index 0000000..a72fa33
--- /dev/null
+++ b/src/particle_filters/resample.jl
@@ -0,0 +1,66 @@
+function resample_systematic(w,M)
+    N = size(w,1)
+    bins = cumsum(exp(w)) # devec
+    s = collect((rand()/M+0):1/M:bins[end])
+    j = Array(Int64,M)
+    bo = 1
+    for i = 1:M
+        @inbounds for b = bo:N
+            if s[i] < bins[b]
+                j[i] = b
+                bo = b
+                break
+            end
+        end
+    end
+    return j
+end
+
+function resample_systematic!(j,w,M)
+    N = size(w,1)
+    bins = cumsum(exp(w)) # devec
+    s = collect((rand()/M+0):1/M:bins[end])
+    bo = 1
+    for i = 1:M
+        @inbounds for b = bo:N
+            if s[i] < bins[b]
+                j[i] = b
+                bo = b
+                break
+            end
+        end
+    end
+    return nothing
+end
+
+function resample_systematic_exp(w,M)
+    N = size(w,1)
+    bins = cumsum(w) # devec
+    s = collect((rand()/M+0):1/M:bins[end])
+    j = Array(Int64,M)
+    bo = 1
+    for i = 1:M
+        for b = bo:N
+            if s[i] <= bins[b]
+                j[i] = b
+                bo = b
+                break
+            end
+        end
+    end
+    return j
+end
+
+# """
+# There is probably lots of room for improvement here. All bins need not be formed in the beginning.
+# One only has to keep 1 values, the current upper limit, no array needed.
+# """
+function draw_one_categorical(w)
+    bins = cumsum(w) # devec
+    s = rand()*bins[end]
+    for b = 1:length(bins)
+        if s < bins[b]
+            return b
+        end
+    end
+end
diff --git a/src/particle_filters/utilities.jl b/src/particle_filters/utilities.jl
new file mode 100644
index 0000000..06c9091
--- /dev/null
+++ b/src/particle_filters/utilities.jl
@@ -0,0 +1,40 @@
+function weighted_mean(xp,w)
+    N,T = size(xp)
+    xh = zeros(Float64,T)
+    for j = 1:T
+        @inbounds for i = 1:N
+            xh[j] += xp[i,j]*exp(w[i,j])
+        end
+    end
+    return xh
+end
+
+function weighted_mean!(xh,xp,w)
+    N,T = size(xp)
+    for j = 1:T
+        @inbounds for i = 1:N
+            xh[j] += xp[i,j]*exp(w[i,j])
+        end
+    end
+    return xh
+end
+
+
+function mode(xp,w)
+    N,T = size(xp)
+    xh = zeros(Float64,T)
+    for j = 1:T
+        i = indmax(slice(w,:,j))
+        xh[j] = xp[i,j]
+    end
+    return xh
+end
+
+function mode!(xh,xp,w)
+    N,T = size(xp)
+    for j = 1:T
+        i = indmax(slice(w,:,j))
+        xh[j] = xp[i,j]
+    end
+    return xh
+end
-- 
GitLab