Commit 2d7d3c96 authored by Mattias Fält's avatar Mattias Fält
Browse files

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<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
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!
Please register or to comment