Commit 2d7d3c96 by Mattias Fält

### Initial SQMC commit with own PF

parent 9fcbbe36
 #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
 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
 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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!