Initial SQMC commit with own PF

src/hilbert_curve.jl

+#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

src/particle_filters/SQMC_test.jl

+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

src/particle_filters/pf_SQMC.jl

+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