From 2d7d3c96fcf245403070bed6223f72d82dc7a319 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mattias=20F=C3=A4lt?= <mattiasf@control.lth.se>
Date: Tue, 1 Dec 2015 18:09:07 +0100
Subject: [PATCH] Initial SQMC commit with own PF
---
src/hilbert_curve.jl | 62 ++++++++++++
src/particle_filters/SQMC_test.jl | 161 ++++++++++++++++++++++++++++++
src/particle_filters/pf_SQMC.jl | 108 ++++++++++++++++++++
3 files changed, 331 insertions(+)
create mode 100644 src/hilbert_curve.jl
create mode 100644 src/particle_filters/SQMC_test.jl
create mode 100644 src/particle_filters/pf_SQMC.jl
diff --git a/src/hilbert_curve.jl b/src/hilbert_curve.jl
new file mode 100644
index 0000000..bde607a
--- /dev/null
+++ b/src/hilbert_curve.jl
@@ -0,0 +1,62 @@
+#Convert number in [0,1)^2 to [0,1)
+function xy2d(x::Float64, y:: Float64)
+ N = 2^30
+ xint = floor(Int64, x*N)
+ yint = floor(Int64, y*N)
+ # d will be in [0,N^2)
+ d = xy2d(N, xint, yint)/N^2
+end
+
+function d2xy(d:: Float64)
+ N::Int64 = 2^30
+ dint = floor(Int64, d*N^2)
+ x::Float64, y::Float64 = d2xy(N^2, dint)
+ x/N, y/N
+end
+
+#Converted from wikipedia
+#convert (x,y) to d
+function xy2d(n::Integer, x::Integer, y::Integer)
+ rx, ry, s, d = 0, 0, 0, 0
+ s = div(n,2)
+ while s>0
+ rx = (x & s) > 0
+ ry = (y & s) > 0
+ d += s * s * ((3*rx) $ ry)
+ x, y = rot(s, x, y, rx, ry)
+ s = div(s,2)
+ end
+ return d::Integer
+end
+
+#convert d to (x,y)
+function d2xy(n::Integer, d::Integer)
+ rx, ry, s, t = 0, 0, 0, d
+ x, y = 0, 0
+ s = 1
+ while s<n
+ rx = 1 & (div(t,2))
+ ry = 1 & (t $ rx)
+ x, y = rot(s, x, y, rx, ry)
+ x += s*rx
+ y += s*ry
+ t = div(t,4)
+ s *= 2
+ end
+ return x::Integer, y::Integer
+end
+
+#rotate/flip a quadrant appropriately
+function rot(n::Integer, x::Integer, y::Integer, rx::Integer, ry::Integer)
+ if ry == 0
+ if rx == 1
+ x = n-1 - x
+ y = n-1 - y
+ end
+ #Swap x and y
+ t = x
+ x = y
+ y = t
+ end
+ return x::Integer, y::Integer
+end
diff --git a/src/particle_filters/SQMC_test.jl b/src/particle_filters/SQMC_test.jl
new file mode 100644
index 0000000..e1597a6
--- /dev/null
+++ b/src/particle_filters/SQMC_test.jl
@@ -0,0 +1,161 @@
+using Gadfly
+using Colors
+
+function GordonKitagawaUpdate!(x,t, u, xtemp, i)
+ b₁, b₂, b₃, b₄, σ = .5, 25, 8, 1.2, sqrt(1);
+ xtemp[1,i] = b₁*x+b₂*x/(t+x^2)+b₃*cos(b₄*t)+σ*erfinv(u[1]*2-1);
+end
+
+function GordonKitagawaOut(x)
+ σ = sqrt(.1);
+ a = 20
+ x^2/a+σ*randn()[1]
+end
+
+function GordonKitagawaOutNoNoise(x)
+ a = 20
+ x.^2./a
+end
+
+function GordonKitagawapxy(yhat,y)
+ σ = sqrt(.1);
+ #w = 1/sqrt(2π)*e^(-(yhat[1]-y[1])^2/2)
+ w = -(yhat[1]-y[1])^2/(2*σ^2)
+end
+
+
+
+function generateRealSequence(f!, g, x0, T, N = length(x0))
+ x = Array{Float64,2}(length(x0), T)
+ f!(x0, 1, rand(N),x,1)
+ y1 = g(x[:,1],1)
+ y = Array{Float64,2}(length(y1), T)
+ y[:,1] = y1
+ for t = 2:T
+ f!(x[t-1], t, rand(N),x,t)
+ y[:,t] = g(x[t], t)
+ end
+ x, y
+end
+
+function estMean(x,w,t)
+ sum(x[:].*exp(w[:]))
+end
+
+function runTest(N, method, debug)
+ T = 150
+
+ ##Gordon Kitagawa
+ #f! = (x,t,u,xtemp,i) -> GordonKitagawaUpdate!(x[1],t,u,xtemp,i)
+ #g = (x,t) -> GordonKitagawaOutNoNoise(x[1])
+ #gn = (x,t) -> GordonKitagawaOut(x[1])
+ #ginv = (yhat, y,t) -> GordonKitagawapxy(yhat,y)
+
+ ##LTI System
+ σ = .5
+ f! = (x,t,u,xtemp,i) -> xtemp[1,i] = .8x[1]+4*erfinv(u[1]*2-1)
+ g = (x,t) -> 2*x[1]
+ gn = (x,t) -> 2*x[1] + σ*randn()[1]
+ ginv = (yhat, y, t) -> -(yhat[1]-y[1])^2/(2*σ^2)
+
+ ##Estimator
+ #est = (x,w,t) -> x[findmax(w)[2]]
+ est = (x,w,t) -> sum(x[:].*exp(w[:]))
+
+ x0 = .5
+ x, y = generateRealSequence(f!, gn, x0, T)
+ xhat = method(f!, g, ginv, x0, y, est, N, debug=debug, xreal=x)
+ x, xhat
+end
+
+
+
+function plotPoints(x, w, y, N, a, τ, t, xreal, xhat)
+ c = w[:]-minimum(w)+1
+ ##Use for GordonK
+ #p = plot(layer(x=collect(1:N), y=x[:], Geom.point, color=c),
+ # layer(x=[1,N],y=ones(2).*sqrt(20*max(y[:,t],0)),Geom.line, Theme(default_color=color(colorant"red"))),
+ # layer(x=[1,N],y=-ones(2).*sqrt(20*max(y[:,t],0)),Geom.line, Theme(default_color=color(colorant"red"))),
+ # layer(x=[1,N],y=ones(2).*xreal[:,t],Geom.line, Theme(default_color=color(colorant"blue"),line_width=2px)),
+ # layer(x=[1,N],y=ones(2).*xhat[:,t],Geom.line, Theme(default_color=color(colorant"black"),line_width=4px)),
+ # Guide.XLabel("Particle "*string(t)), Guide.YLabel("Estimate"), Coord.Cartesian(ymin=-15,ymax=15))
+ ##Use for LTI
+ p = plot(layer(x=collect(1:N), y=x[:], Geom.point, color=c),
+ layer(x=[1,N],y=ones(2)*1/2.*y[:,t],Geom.line, Theme(default_color=color(colorant"red"))),
+ layer(x=[1,N],y=ones(2).*xreal[:,t],Geom.line, Theme(default_color=color(colorant"blue"),line_width=2px)),
+ layer(x=[1,N],y=ones(2).*xhat[:,t],Geom.line, Theme(default_color=color(colorant"black"),line_width=4px)),
+ Guide.XLabel("Particle "*string(t)), Guide.YLabel("Estimate"), Coord.Cartesian(ymin=-10,ymax=10))
+
+ display(p)
+ print("here")
+ readline(STDIN)
+end
+
+
+function rms(x)
+ sqrt(1/length(x)*sum(x.^2))
+end
+
+function testSQMC()
+ debug = false
+ Ns = 2.^(2:11)
+ M = 50
+ RMS = Array{Float64,2}(length(Ns),M)
+ largeRMS = Array{Float64}(length(Ns),2)
+ rmsMean = Array{Float64,2}(length(Ns),2)
+ rmsVariance = Array{Float64,2}(length(Ns),2)
+ for (methodidx,method) in enumerate([SQMC,pf])
+ xhat, xreal = 0, 0
+ @time for (i, N) in enumerate(Ns)
+ rmslocal = Array{Float64,1}(M)
+ for j = 1:M
+ xreal, xhat = runTest(N,method,debug)
+ RMS[i,j] = rms(xreal-xhat)
+ end
+ end
+ rmsMean[:,methodidx] = mean(RMS,2)
+ rmsVariance[:,methodidx] = std(RMS,2)./sqrt(M)
+ for (i, N) in enumerate(Ns)
+ largeRMS[i,methodidx] = length(find(RMS[i,:].>rmsMean[i,methodidx]+2*rmsVariance[i,methodidx]))
+ end
+ end
+ rmsMean, rmsVariance, Ns, largeRMS, RMS
+end
+
+function testPlot(rmsMean, rmsVariance, Ns)
+ p = plot(
+ layer(x=Ns,y=rmsMean[:,1],Geom.line,Theme(default_color=color(colorant"red"))),
+ layer(x=Ns,y=rmsMean[:,2],Geom.line,Theme(default_color=color(colorant"blue"))),
+ layer(x=Ns,y=rmsMean[:,1]+rmsVariance[:,1]*2,Geom.line,Theme(default_color=color(colorant"red"))),
+ layer(x=Ns,y=rmsMean[:,1]-rmsVariance[:,1]*2,Geom.line,Theme(default_color=color(colorant"red"))),
+ layer(x=Ns,y=rmsMean[:,2]+rmsVariance[:,2]*2,Geom.line,Theme(default_color=color(colorant"blue"))),
+ layer(x=Ns,y=rmsMean[:,2]-rmsVariance[:,2]*2,Geom.line,Theme(default_color=color(colorant"blue"))),
+ )
+end
+
+
+function testPlot(N)
+ for j = 1:N
+ M = 1000
+ vals = Array{Float64,2}(M,2)
+ ds = linspace(rand()/M,1,M)[1:end]
+ for i = 1:M
+ o = d2xy(ds[i])
+ vals[i,:] = [o[1], o[2]]
+ end
+ plot(vals[:,1], vals[:,2])
+ end
+end
+
+function testPlot2(N)
+ for j = 1:N
+ M = 1000
+ vals = Array{Float64,2}(M,2)
+ ds = sort(rand(M))
+ for i = 1:M
+ o = d2xy(ds[i])
+ vals[i,:] = [o[1], o[2]]
+ end
+ plot(vals[:,1], vals[:,2])
+ end
+end
diff --git a/src/particle_filters/pf_SQMC.jl b/src/particle_filters/pf_SQMC.jl
new file mode 100644
index 0000000..0fd7940
--- /dev/null
+++ b/src/particle_filters/pf_SQMC.jl
@@ -0,0 +1,108 @@
+function resample(s, w)
+ # Samples new particles based on their weights. If you find algorithmic optimizations to this routine, please tell me /Bagge)
+ N = length(w)
+ bins = [0.; cumsum(exp(w))]
+ j = zeros(Int64,N)
+ bo = 1
+ for i = 1:N
+ for b = bo:N
+ if bins[b] <= s[i]# < bins[b+1]
+ j[i] = b
+ bo = b
+ break
+ end
+ end
+ end
+ return j
+end
+
+function resample(w)
+ bins = [0.; cumsum(exp(w))]
+ s = collect((rand()/N+0):1/N:bins[end])
+ resample(s,w)
+end
+
+@doc """ `xhat = SQMC(f, g, ginv, x0, y, est, N=200, Nu=lenth(x0))`
+
+ Gives estimation of the states `x(:,1),...x(:,T)` given outputs `y(:,1)...y(:,T)` from system:
+
+ x(t) = f(x(t-1),t,u) \n
+ y = gn(x,v),\n
+ where `g(x,t)=E[gn(x,t,u)]`, `ginv(yhat, y, t) = p(y=gn(x,u)|yhat=g(x))`,
+ u is uniform noise on `[0,1)`, with `E[x(0)] = xhat0`,
+ and some estimator `est(xₚ,w,t)` that ouputs the estimate `xhat(:,t)`
+ given the particles `xₚ` and weights `w`
+ """ ->
+function SQMC(f!, g, ginv, xhat0, y, est, N=200, Nu = length(xhat0); debug = false, xreal=xreal)
+ T = size(y,2)
+ #Estimates at time 1:T
+ xhat = Array{Float64,2}(Nu, T)
+ #Current predicted output for each particle
+ yhat = Array{Float64,2}(size(y,1), N)
+ x = Array{Float64,2}(length(xhat0), N)
+ xtemp = similar(x)
+ w = Array{Float64,1}(N)
+ #Step (a) (Draw u and x), OBS u will be of size NxNu
+ u, = sobol(Nu,N)
+ x[:,1] = xhat0; #Hack to use the pfStep fuction. Works by letting `a` be ones
+ pfStep!(f!,g,ginv,x,xtemp,u,ones(Int,N),1:N,1,w,yhat,y,N, 1)
+ xhat[:,1] = est(x,w,1)
+ u, nextseed, MeM = sobol(Nu+1,N)
+ for t = 2:T
+ nextseed = sobol!(u, nextseed, MeM)
+ τ = sortperm(u[:,1])
+ σ = sortperm(ψ(x)[:])
+ a = resample(u[τ,1], w[σ])
+ # Time update, no sigma here?
+ pfStep!(f!,g,ginv,x,xtemp,u,a,τ,t,w,yhat,y,N, 2:Nu+1)
+ xhat[:,t] = est(x,w,1)
+ if debug
+ plotPoints(x, w, y, N, a, τ, t, xreal, xhat)
+ end
+ end
+ xhat
+end
+
+
+function ψ(x)
+ x
+end
+
+function pf(f!, g, ginv, xhat0, y, est, N=200, Nu = length(xhat0); debug=false, xreal=xreal)
+ T = size(y,2)
+ #Estimates at time 1:T
+ xhat = Array{Float64,2}(Nu, T)
+ #Current predicted output for each particle
+ yhat = Array{Float64,2}(size(y,1), N)
+ x = Array{Float64,2}(length(xhat0), N)
+ xtemp = similar(x)
+ w = Array{Float64,1}(N)
+ τ = 1:N
+ u = rand(N,Nu)
+ x[:,1] = xhat0; #Hack to use the pfStep fuction. Works by letting `a` be ones
+ pfStep!(f!,g,ginv,x,xtemp,u,ones(Int,N),τ,1,w,yhat,y,N, 1:Nu)
+ xhat[:,1] = est(x,w,1)
+ for t = 2:T
+ u = rand(N,Nu)
+ a = resample(u[τ,1], w[:])
+ # Time update, no sigma here?
+ pfStep!(f!,g,ginv,x,xtemp,u,a,τ,t,w,yhat,y,N,1:Nu)
+ xhat[:,t] = est(x,w,1)
+ end
+ xhat
+end
+
+#Time update and weighting. Chances `x`, `yhat` and `w`
+@inbounds function pfStep!(f,g,ginv,x,xtemp,u,a,τ,t,w,yhat,y,N,uRange)
+ for i = 1:N
+ f(x[:,a[i]], t, u[τ[i], uRange], xtemp, i)
+ yhat[:,i] = g(xtemp[:,i], t)
+ w[i] = ginv(yhat[:,i], y[:,t], t)
+ end
+ x[:] = copy(xtemp)
+
+ offset = maximum(w)
+ mySum = sum(exp(w-offset))
+ normConstant = log(mySum)+offset
+ w[:] = w[:] - normConstant
+end
\ No newline at end of file
--
GitLab