particle_filter_test.jl

# module Tmp
using Devectorize
include("pf_bootstrap.jl")
include("pf_smoother_FFBSi.jl")
include("pf_PMMH.jl")
include("resample.jl")
include("utilities.jl")
const σw0 = 2.0
const σw = 0.32
const σv = 1.0
const theta0 = [0.5, 25, 8, 1.2]
s2piσv = log(sqrt(2*pi) * σv)

function f(x::Vector, t::Int64)
    c = 8*cos(1.2*(t-1))
    @inbounds for i = 1:length(x)
        x[i] =  0.5*x[i] + 25*x[i]./(1+x[i]^2) + c
    end

end
f(x::Float64, t::Int64) = 0.5*x + 25*x/(1+x^2) + 8*cos(1.2*(t-1))

function f_density(xb, xp, t)
    x = f(xp,t)-xb
    exp(-0.5 * (x/σw)^2)
end


const den = 0.5/σv^2
function g_density(y::Float64, x::Vector{Float64}, w::Vector{Float64})
    @inbounds for i = 1:length(w)
        w[i] -= (den * (y-0.05x[i]^2)^2 - s2piσv)
    end
end


## Parameter estimation
function f_theta(x::Vector, t::Int64, theta)
    c = theta[3]*cos(theta[4]*(t-1))
    @inbounds for i = 1:length(x)
        x[i] =  theta[1]*x[i] + theta[2]*x[i]./(1+x[i]^2) + c
    end

end
# f_theta(x::Float64, t::Int64) = theta[1]*x + theta[2]*x/(1+x^2) + theta[3]*cos(1.2*(t-1))

function f_density_theta(xb, xp, t, theta)
    x = f_theta(xp,t, theta)-xb
    exp(-0.5 * (x/σw)^2)
end


function g_density_theta(y::Float64,x::Vector{Float64},w, theta)
    #     w = Array(Float64,size(x))
    @inbounds for i = 1:length(w)
        w[i] = -(den * (y-0.05x[i]^2)^2)
    end
    return w
end

draw_theta(theta) = theta + 0.05*randn(size(theta0)).*abs(theta0)
draw_theta0() = theta0+ 0.1*randn(size(theta0)).*abs(theta0)



rms(x) = sqrt(mean(x.^2))

function test_pf()
    particle_count = [5 15 50 150 500 3000 10_000]
    time_steps = [100, 2000]
    RMSE = zeros(length(particle_count),length(time_steps)) # Store the RMS errors
    RMSE_FFBSi = zeros(length(particle_count),length(time_steps))
    propagated_particles = 0
    M = 10
    for (Ti,T) in enumerate(time_steps)
        xh = Array(Float64,T)
        for (Ni, N) in enumerate(particle_count)
            montecarlo_runs = maximum(particle_count)*maximum(time_steps) / T / N*10
            #             montecarlo_runs = 1
            xp = Array(Float64,(N,T))
            w = Array(Float64,(N,T))
            x = Array(Float64,T)
            y = Array(Float64,T)

            for mc_iter = 1:montecarlo_runs
                x[1] = σw*randn()
                y[1] = σv*randn()
                for t = 1:T-1
                    x[t+1] = f(x[t],t) + σw*randn()
                    y[t+1] = 0.05x[t+1]^2  + σv*randn()
                end # t
                pf_bootstrap!(xp,w, y, N, g_density, f, σw0 )
                xb = pf_smoother_FFBSi( w,xp, M, f_density )
                #                 weighted_mean!(xh,xp,w)
                mode!(xh,xp,w)
                RMSE[Ni,Ti] += rms(x-xh)
                RMSE_FFBSi[Ni,Ti] += rms(x-mean(xb,1)')
            end # MC
            RMSE[Ni,Ti] /= montecarlo_runs
            RMSE_FFBSi[Ni,Ti] /= montecarlo_runs
            propagated_particles += montecarlo_runs*N*T
            @show N
        end # N
        @show T
    end # T
    println("Propagated $propagated_particles particles")
    #
    return RMSE, RMSE_FFBSi
end

using Winston
function test_PMMH()
    particle_count = [100]
    time_steps = [200]
    R = 10_000
    for (Ti,T) in enumerate(time_steps)
        for (Ni, N) in enumerate(particle_count)
            montecarlo_runs = 16# maximum(particle_count)*maximum(time_steps) / T / N*10
            x = Array(Float64,T)
            y = Array(Float64,T)
            theta = SharedArray(Float64,(size(theta0,1),R,montecarlo_runs))
            @sync @parallel for mc_iter = 1:montecarlo_runs
                x[1] = σw*randn()
                y[1] = σv*randn()
                for t = 1:T-1
                    x[t+1] = f(x[t],t) + σw*randn()
                    y[t+1] = 0.05x[t+1]^2  + σv*randn()
                end # t

                theta[:,:,mc_iter] = pf_PMMH(y, N, R, pf_bootstrap_nn, f_theta, draw_theta, f_density_theta, g_density_theta, σw0, draw_theta0())
            end # MC
            newplot(theta[:,:,1]'); hold(true)
            for mc_iter = 2:montecarlo_runs
                plot(theta[:,:,mc_iter]')
            end
            hold(false)
            @show N
        end # N
        @show T
    end # T
    println("Done test_PMMH")
    #     return theta
end


pf_bootstrap!(eye(4),eye(4), ones(4), 4, g_density, f, σw )
gc()
Profile.clear()


function plotting(RMSE)
    particle_count = [5 15 50 150 500 3000 10_000]
    time_steps = [100, 2000]
    nT = length(time_steps)
    p = loglog(particle_count,RMSE,"o")
    legend_strings = Array(String,nT)
    for i = 1:nT
        legend_strings[i] = "$(time_steps[i]) time steps"
    end
    title("RMS errors vs Number of particles")
    @windows_only display(p)
    @linux_only PYPLOT && legend(legend_strings)
    #     ProfileView.view()
end