diff --git a/.travis.yml b/.travis.yml index d3e2079..daedcb7 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,15 +4,16 @@ os: - linux # - osx julia: - - 0.6 + - 0.7 + - 1.0 notifications: email: false # uncomment the following lines to override the default test script -script: - - if [[ -a .git/shallow ]]; then git fetch --unshallow; fi - - julia -e 'Pkg.clone(pwd()); Pkg.build("ParticleFilters")' - # - julia -e 'include(Pkg.dir("ParticleFilters", "test", "build.jl"))' - - julia -e 'Pkg.test("ParticleFilters"; coverage=true)' +# script: +# - if [[ -a .git/shallow ]]; then git fetch --unshallow; fi +# - julia -e 'Pkg.clone(pwd()); Pkg.build("ParticleFilters")' +# # - julia -e 'include(Pkg.dir("ParticleFilters", "test", "build.jl"))' +# - julia -e 'Pkg.test("ParticleFilters"; coverage=true)' after_success: # push coverage results to Coveralls - julia -e 'cd(Pkg.dir("ParticleFilters")); Pkg.add("Coverage"); using Coverage; Coveralls.submit(Coveralls.process_folder())' diff --git a/README.md b/README.md index 9882487..6018f24 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,7 @@ For example, a double integrator model (written for clarity, not speed) is shown ```julia using ParticleFilters +using LinearAlgebra using Distributions using Reel using Plots @@ -63,7 +64,7 @@ function ParticleFilters.observation(model::DblIntegrator2D, u, sp) end N = 1000 -model = DblIntegrator2D(0.001*eye(4), eye(2), 0.1) +model = DblIntegrator2D(0.001*Diagonal{Float64}(I, 4), Diagonal{Float64}(I, 2), 0.1) filter = SIRParticleFilter(model, N) srand(1) rng = Base.GLOBAL_RNG diff --git a/REQUIRE b/REQUIRE index 3bb8072..68f759a 100644 --- a/REQUIRE +++ b/REQUIRE @@ -1,4 +1,5 @@ julia 0.6 StatsBase POMDPs 0.6 -POMDPToolbox 0.2.7 +POMDPModelTools +POMDPPolicies diff --git a/src/ParticleFilters.jl b/src/ParticleFilters.jl index 4c892a8..098ffb4 100644 --- a/src/ParticleFilters.jl +++ b/src/ParticleFilters.jl @@ -3,17 +3,21 @@ __precompile__() module ParticleFilters using POMDPs -import POMDPs: pdf, mode, update, initialize_belief, iterator -import POMDPs: state_type, isterminal, observation +import POMDPs: pdf, mode, update, initialize_belief, support +import POMDPs: statetype, isterminal, observation import POMDPs: generate_s import POMDPs: action, value import POMDPs: implemented import POMDPs: sampletype -import Base: rand, mean -using POMDPToolbox -import POMDPToolbox: obs_weight +import POMDPModelTools: obs_weight using StatsBase +using Random +using Statistics +using POMDPPolicies + +import Random: rand +import Statistics: mean export AbstractParticleBelief, @@ -40,18 +44,19 @@ export pdf, mode, update, + support, initialize_belief export generate_s, observation, isterminal, - state_type + statetype abstract type AbstractParticleBelief{T} end # DEPRECATED: remove in future release -Base.eltype{T}(::Type{AbstractParticleBelief{T}}) = T +Base.eltype(::Type{AbstractParticleBelief{T}}) where {T} = T sampletype(::Type{B}) where B<:AbstractParticleBelief{T} where T = T @@ -62,21 +67,21 @@ Unweighted particle belief """ mutable struct ParticleCollection{T} <: AbstractParticleBelief{T} particles::Vector{T} - _probs::Nullable{Dict{T,Float64}} # a cache for the probabilities + _probs::Union{Nothing, Dict{T,Float64}} # a cache for the probabilities ParticleCollection{T}() where {T} = new(T[], nothing) - ParticleCollection{T}(particles) where {T} = new(particles, Nullable{Dict{T,Float64}}()) + ParticleCollection{T}(particles) where {T} = new(particles, Dict{T,Float64}()) ParticleCollection{T}(particles, _probs) where {T} = new(particles, _probs) end -ParticleCollection{T}(p::AbstractVector{T}) = ParticleCollection{T}(p, nothing) +ParticleCollection(p::AbstractVector{T}) where T = ParticleCollection{T}(p, nothing) mutable struct WeightedParticleBelief{T} <: AbstractParticleBelief{T} particles::Vector{T} weights::Vector{Float64} weight_sum::Float64 - _probs::Nullable{Dict{T,Float64}} # this is not used now, but may be later + _probs::Union{Nothing, Dict{T,Float64}} # this is not used now, but may be later end -WeightedParticleBelief{T}(particles::AbstractVector{T}, weights::AbstractVector, weight_sum=sum(weights)) = WeightedParticleBelief{T}(particles, weights, weight_sum, nothing) +WeightedParticleBelief(particles::AbstractVector{T}, weights::AbstractVector, weight_sum=sum(weights)) where {T} = WeightedParticleBelief{T}(particles, weights, weight_sum, nothing) ### Belief interface ### # see beliefs.jl for implementation @@ -141,14 +146,14 @@ mutable struct SimpleParticleFilter{S,M,R,RNG<:AbstractRNG} <: Updater _particle_memory::Vector{S} _weight_memory::Vector{Float64} - SimpleParticleFilter{S, M, R, RNG}(model, resample, rng) where {S,M,R,RNG} = new(model, resample, rng, state_type(model)[], Float64[]) + SimpleParticleFilter{S, M, R, RNG}(model, resample, rng) where {S,M,R,RNG} = new(model, resample, rng, statetype(model)[], Float64[]) end -function SimpleParticleFilter{R}(model, resample::R, rng::AbstractRNG) - SimpleParticleFilter{state_type(model),typeof(model),R,typeof(rng)}(model, resample, rng) +function SimpleParticleFilter(model, resample::R, rng::AbstractRNG) where {R} + SimpleParticleFilter{statetype(model),typeof(model),R,typeof(rng)}(model, resample, rng) end -SimpleParticleFilter(model, resample; rng::AbstractRNG=Base.GLOBAL_RNG) = SimpleParticleFilter(model, resample, rng) +SimpleParticleFilter(model, resample; rng::AbstractRNG=Random.GLOBAL_RNG) = SimpleParticleFilter(model, resample, rng) -function update{S}(up::SimpleParticleFilter{S}, b::ParticleCollection, a, o) +function update(up::SimpleParticleFilter{S}, b::ParticleCollection, a, o) where {S} ps = particles(b) pm = up._particle_memory wm = up._weight_memory @@ -173,14 +178,14 @@ function update{S}(up::SimpleParticleFilter{S}, b::ParticleCollection, a, o) return resample(up.resample, WeightedParticleBelief{S}(pm, wm, sum(wm), nothing), up.rng) end -function Base.srand(f::SimpleParticleFilter, seed) - srand(f.rng, seed) +function Random.seed!(f::SimpleParticleFilter, seed) + Random.seed!(f.rng, seed) return f end # default for non-POMDPs -state_type(model) = Any +statetype(model) = Any isterminal(model, s) = false observation(model, s, a, sp) = observation(model, a, sp) @@ -206,7 +211,7 @@ function resample end ### Convenience Aliases ### const SIRParticleFilter{T} = SimpleParticleFilter{T, LowVarianceResampler} -function SIRParticleFilter(model, n::Int; rng::AbstractRNG=Base.GLOBAL_RNG) +function SIRParticleFilter(model, n::Int; rng::AbstractRNG=Random.GLOBAL_RNG) return SimpleParticleFilter(model, LowVarianceResampler(n), rng) end diff --git a/src/beliefs.jl b/src/beliefs.jl index b6d7cb8..79478c7 100644 --- a/src/beliefs.jl +++ b/src/beliefs.jl @@ -5,8 +5,8 @@ weight_sum(::ParticleCollection) = 1.0 weight(b::ParticleCollection, i::Int) = 1.0/length(b.particles) particle(b::ParticleCollection, i::Int) = b.particles[i] rand(rng::AbstractRNG, b::ParticleCollection) = b.particles[rand(rng, 1:length(b.particles))] -mean(b::ParticleCollection) = sum(b.particles)/length(b.particles) -iterator(b::ParticleCollection) = particles(b) +Statistics.mean(b::ParticleCollection) = sum(b.particles)/length(b.particles) +support(b::ParticleCollection) = unique(particles(b)) n_particles(b::WeightedParticleBelief) = length(b.particles) particles(p::WeightedParticleBelief) = p.particles @@ -16,7 +16,7 @@ weight(b::WeightedParticleBelief, i::Int) = b.weights[i] particle(b::WeightedParticleBelief, i::Int) = b.particles[i] weights(b::WeightedParticleBelief) = b.weights -function rand(rng::AbstractRNG, b::WeightedParticleBelief) +function Random.rand(rng::AbstractRNG, b::WeightedParticleBelief) t = rand(rng) * weight_sum(b) i = 1 cw = b.weights[1] @@ -26,10 +26,10 @@ function rand(rng::AbstractRNG, b::WeightedParticleBelief) end return particles(b)[i] end -mean(b::WeightedParticleBelief) = dot(b.weights, b.particles)/weight_sum(b) +Statistics.mean(b::WeightedParticleBelief) = dot(b.weights, b.particles)/weight_sum(b) -function get_probs{S}(b::AbstractParticleBelief{S}) - if isnull(b._probs) +function get_probs(b::AbstractParticleBelief{S}) where {S} + if b._probs == nothing # update the cache probs = Dict{S, Float64}() for (i,p) in enumerate(particles(b)) @@ -39,14 +39,14 @@ function get_probs{S}(b::AbstractParticleBelief{S}) probs[p] = weight(b, i)/weight_sum(b) end end - b._probs = Nullable(probs) + b._probs = probs end - return get(b._probs) + return b._probs end -pdf{S}(b::AbstractParticleBelief{S}, s::S) = get(get_probs(b), s, 0.0) +pdf(b::AbstractParticleBelief{S}, s::S) where {S} = get(get_probs(b), s, 0.0) -function mode{T}(b::AbstractParticleBelief{T}) # don't know if this is efficient +function mode(b::AbstractParticleBelief{T}) where {T} # don't know if this is efficient probs = get_probs(b) best_weight = 0.0 most_likely = first(keys(probs)) @@ -59,4 +59,4 @@ function mode{T}(b::AbstractParticleBelief{T}) # don't know if this is efficient return most_likely end -iterator(b::AbstractParticleBelief) = keys(get_probs(b)) +support(b::AbstractParticleBelief) = keys(get_probs(b)) diff --git a/src/policies.jl b/src/policies.jl index 687d72c..b1c9581 100644 --- a/src/policies.jl +++ b/src/policies.jl @@ -11,7 +11,7 @@ the corresponding particle function unnormalized_util(p::AlphaVectorPolicy, b::AbstractParticleBelief) util = zeros(n_actions(p.pomdp)) for (i, s) in enumerate(particles(b)) - j = state_index(p.pomdp, s) + j = stateindex(p.pomdp, s) util += weight(b, i)*getindex.(p.alphas, (j,)) end return util @@ -19,7 +19,7 @@ end function action(p::AlphaVectorPolicy, b::AbstractParticleBelief) util = unnormalized_util(p, b) - ihi = indmax(util) + ihi = argmax(util) return p.action_map[ihi] end diff --git a/src/resamplers.jl b/src/resamplers.jl index 489b43d..f7b2fcb 100644 --- a/src/resamplers.jl +++ b/src/resamplers.jl @@ -1,4 +1,4 @@ -function resample{S}(r::ImportanceResampler, b::WeightedParticleBelief{S}, rng::AbstractRNG) +function resample(r::ImportanceResampler, b::WeightedParticleBelief{S}, rng::AbstractRNG) where {S} ps = Array{S}(r.n) if weight_sum(b) <= 0 warn("Invalid weights in particle filter: weight_sum = $(weight_sum(b))") @@ -8,8 +8,8 @@ function resample{S}(r::ImportanceResampler, b::WeightedParticleBelief{S}, rng:: return ParticleCollection(ps) end -function resample{S}(re::LowVarianceResampler, b::AbstractParticleBelief{S}, rng::AbstractRNG) - ps = Array{S}(re.n) +function resample(re::LowVarianceResampler, b::AbstractParticleBelief{S}, rng::AbstractRNG) where {S} + ps = Array{S}(undef, re.n) r = rand(rng)*weight_sum(b)/re.n c = weight(b,1) i = 1 @@ -25,10 +25,10 @@ function resample{S}(re::LowVarianceResampler, b::AbstractParticleBelief{S}, rng return ParticleCollection(ps) end -function resample{S}(re::LowVarianceResampler, b::ParticleCollection{S}, rng::AbstractRNG) +function resample(re::LowVarianceResampler, b::ParticleCollection{S}, rng::AbstractRNG) where {S} r = rand(rng)*n_particles(b)/re.n chunk = n_particles(b)/re.n - inds = ceil.(Int, chunk*(0:re.n-1)+r) + inds = ceil.(Int, chunk*(0:re.n-1).+r) ps = particles(b)[inds] return ParticleCollection(ps) end @@ -36,7 +36,7 @@ end resample(r::Union{ImportanceResampler,LowVarianceResampler}, b, rng::AbstractRNG) = resample(r, b, sampletype(b), rng) function resample(r::Union{ImportanceResampler,LowVarianceResampler}, b, sampletype::Type, rng::AbstractRNG) - ps = Array{sampletype}(r.n) + ps = Array{sampletype}(undef, r.n) for i in 1:r.n ps[i] = rand(rng, b) end diff --git a/src/updater.jl b/src/updater.jl index e8e5f3a..95992b8 100644 --- a/src/updater.jl +++ b/src/updater.jl @@ -1,3 +1,3 @@ -initialize_belief{S}(up::SimpleParticleFilter{S}, d::Any) = resample(up.resample, d, S, up.rng) +initialize_belief(up::SimpleParticleFilter{S}, d::Any) where {S} = resample(up.resample, d, S, up.rng) resample(f::Function, d::Any, rng::AbstractRNG) = f(d, rng) diff --git a/test/example.jl b/test/example.jl index 8b42676..e1d1516 100644 --- a/test/example.jl +++ b/test/example.jl @@ -1,6 +1,8 @@ using ParticleFilters using Distributions using StaticArrays +using LinearAlgebra +using Random struct DblIntegrator2D W::Matrix{Float64} # Process noise covariance @@ -22,22 +24,24 @@ function ParticleFilters.observation(model::DblIntegrator2D, a, sp) return MvNormal(sp[1:2], model.V) end -N = 1000 -model = DblIntegrator2D(0.001*eye(4), eye(2), 0.1) -filter = SIRParticleFilter(model, N) -srand(1) -rng = Base.GLOBAL_RNG -b = ParticleCollection([4.0*rand(4)-2.0 for i in 1:N]) -s = [0.0, 1.0, 1.0, 0.0] -for i in 1:100 - global b, s; print(".") - m = mean(b) - a = [-m[1], -m[2]] # try to orbit the origin - s = generate_s(model, s, a, rng) - o = rand(observation(model, a, s)) - b = update(filter, b, a, o) +@testset "example" begin + N = 1000 + model = DblIntegrator2D(0.001*Diagonal{Float64}(I, 4), Diagonal{Float64}(I, 2), 0.1) + filter = SIRParticleFilter(model, N) + Random.seed!(1) + rng = Random.GLOBAL_RNG + b = ParticleCollection([4.0*rand(4).-2.0 for i in 1:N]) + s = [0.0, 1.0, 1.0, 0.0] + for i in 1:100 + print(".") + m = mean(b) + a = [-m[1], -m[2]] # try to orbit the origin + s = generate_s(model, s, a, rng) + o = rand(observation(model, a, s)) + b = update(filter, b, a, o) - # scatter([p[1] for p in particles(b)], [p[2] for p in particles(b)], color=:black, markersize=0.1, label="") - # scatter!([s[1]], [s[2]], color=:blue, xlim=(-5,5), ylim=(-5,5), title=t, label="") + # scatter([p[1] for p in particles(b)], [p[2] for p in particles(b)], color=:black, markersize=0.1, label="") + # scatter!([s[1]], [s[2]], color=:blue, xlim=(-5,5), ylim=(-5,5), title=t, label="") + end + # write("particles.gif", film) end -# write("particles.gif", film) diff --git a/test/runtests.jl b/test/runtests.jl index ffe5692..6bdba11 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,72 +1,85 @@ using ParticleFilters using POMDPs using POMDPModels -using POMDPToolbox -using Base.Test - +using Test +using POMDPPolicies import ParticleFilters: obs_weight import POMDPs: observation -struct P <: POMDP{Void, Void, Void} end - -@test !@implemented obs_weight(::P, ::Void, ::Void, ::Void, ::Void) -@test !@implemented obs_weight(::P, ::Void, ::Void, ::Void) -@test !@implemented obs_weight(::P, ::Void, ::Void) - -obs_weight(::P, ::Void, ::Void, ::Void) = 1.0 -@test @implemented obs_weight(::P, ::Void, ::Void, ::Void) -@test @implemented obs_weight(::P, ::Void, ::Void, ::Void, ::Void) -@test !@implemented obs_weight(::P, ::Void, ::Void) +struct P <: POMDP{Nothing, Nothing, Nothing} end +@testset "!implemented" begin + @test !@implemented obs_weight(::P, ::Nothing, ::Nothing, ::Nothing, ::Nothing) + @test !@implemented obs_weight(::P, ::Nothing, ::Nothing, ::Nothing) + @test !@implemented obs_weight(::P, ::Nothing, ::Nothing) +end +obs_weight(::P, ::Nothing, ::Nothing, ::Nothing) = 1.0 -@test obs_weight(P(), nothing, nothing, nothing, nothing) == 1.0 +@testset "implemented" begin + @test @implemented obs_weight(::P, ::Nothing, ::Nothing, ::Nothing) + @test @implemented obs_weight(::P, ::Nothing, ::Nothing, ::Nothing, ::Nothing) + @test !@implemented obs_weight(::P, ::Nothing, ::Nothing) + @test obs_weight(P(), nothing, nothing, nothing, nothing) == 1.0 +end -observation(::P, ::Void) = nothing -@test @implemented obs_weight(::P, ::Void, ::Void) +observation(::P, ::Nothing) = nothing +@test @implemented obs_weight(::P, ::Nothing, ::Nothing) include("example.jl") -p = TigerPOMDP() -filter = SIRParticleFilter(p, 10000) -srand(filter, 47) -b = @inferred initialize_belief(filter, initial_state_distribution(p)) -m = @inferred mode(b) -m = @inferred mean(b) -it = @inferred iterator(b) -@inferred weighted_particles(b) - -rs = LowVarianceResampler(1000) -@inferred resample(rs, b, MersenneTwister(3)) -ps = particles(b) -ws = ones(length(ps)) -@inferred resample(rs, WeightedParticleBelief(ps, ws, sum(ws)), MersenneTwister(3)) -@inferred resample(rs, WeightedParticleBelief{Bool}(ps, ws, sum(ws), nothing), MersenneTwister(3)) - -# test that the special method for ParticleCollections works -b = ParticleCollection(1:1000) -rb1 = @inferred resample(rs, b, MersenneTwister(3)) -rb2 = @inferred resample(rs, WeightedParticleBelief(particles(b), ones(n_particles(b))), MersenneTwister(3)) -@test all(particles(rb1).==particles(rb2)) - -rng = MersenneTwister(47) -uf = UnweightedParticleFilter(p, 1000, rng) -ps = @inferred initialize_belief(uf, initial_state_distribution(p)) -a = @inferred rand(rng, actions(p)) -sp, o = @inferred generate_so(p, rand(rng, ps), a, rng) -bp = @inferred update(uf, ps, a, o) +@testset "infer" begin + p = TigerPOMDP() + filter = SIRParticleFilter(p, 10000) + Random.seed!(filter, 47) + b = @inferred initialize_belief(filter, initialstate_distribution(p)) + @testset "sir" begin + m = @inferred mode(b) + m = @inferred mean(b) + it = @inferred support(b) + @inferred weighted_particles(b) + end + @testset "lowvar" begin + rs = LowVarianceResampler(1000) + @inferred resample(rs, b, MersenneTwister(3)) + ps = particles(b) + ws = ones(length(ps)) + @inferred resample(rs, WeightedParticleBelief(ps, ws, sum(ws)), MersenneTwister(3)) + @inferred resample(rs, WeightedParticleBelief{Bool}(ps, ws, sum(ws), nothing), MersenneTwister(3)) + end + # test that the special method for ParticleCollections works + @testset "collection" begin + rs = LowVarianceResampler(1000) + b = ParticleCollection(1:1000) + rb1 = @inferred resample(rs, b, MersenneTwister(3)) + rb2 = @inferred resample(rs, WeightedParticleBelief(particles(b), ones(n_particles(b))), MersenneTwister(3)) + @test all(particles(rb1).==particles(rb2)) + end + + @testset "unweighted" begin + rng = MersenneTwister(47) + uf = UnweightedParticleFilter(p, 1000, rng) + ps = @inferred initialize_belief(uf, initialstate_distribution(p)) + a = @inferred rand(rng, actions(p)) + sp, o = @inferred generate_so(p, rand(rng, ps), a, rng) + bp = @inferred update(uf, ps, a, o) -wp1 = @inferred collect(weighted_particles(ParticleCollection([1,2]))) -wp2 = @inferred collect(weighted_particles(WeightedParticleBelief([1,2], [0.5, 0.5]))) -@test wp1 == wp2 + wp1 = @inferred collect(weighted_particles(ParticleCollection([1,2]))) + wp2 = @inferred collect(weighted_particles(WeightedParticleBelief([1,2], [0.5, 0.5]))) + @test wp1 == wp2 + end +end -# test specific method for alpha vector policies and particle beliefs -pomdp = BabyPOMDP() -# these values were gotten from FIB.jl -alphas = [-29.4557 -36.5093; -19.4557 -16.0629] -policy = AlphaVectorPolicy(pomdp, alphas) +@testset "alpha" begin + # test specific method for alpha vector policies and particle beliefs + pomdp = BabyPOMDP() + # these values were gotten from FIB.jl + # alphas = [-29.4557 -36.5093; -19.4557 -16.0629] + alphas = [-16.0629 -19.4557; -36.5093 -29.4557] + policy = AlphaVectorPolicy(pomdp, alphas) -# initial belief is 100% confidence in baby being hungry -b = ParticleCollection([true for i=1:100]) + # initial belief is 100% confidence in baby being hungry + b = ParticleCollection([true for i=1:100]) -# because baby is hungry, policy should feed (return true) -@test action(policy, b) == true -@test isapprox(value(policy, b), -29.4557) + # because baby is hungry, policy should feed (return true) + @test action(policy, b) == true + @test isapprox(value(policy, b), -29.4557) +end