pf_SQMC.jl 3.28 KB
 Mattias Fält committed Dec 01, 2015 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 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``````