LowLevelParticleFilters
This readme is auto generated from the file src/example_lineargaussian.jl using Literate.jl
Types
We provide a number of filter types
ParticleFilter
: This filter is simple to use and assumes that both dynamics noise and measurement noise are additive.AuxiliaryParticleFilter
: This filter is identical toParticleFilter
, but uses a slightly different proposal mechanism for new particles.AdvancedParticleFilter
: This filter gives you more flexibility, at the expense of having to define a few more functions. More instructions on this type below.KalmanFilter
. Is what you would expect. Has the same features as the particle filters, but is restricted to linear dynamics and gaussian noise.UnscentedKalmanFilter
. Is also what you would expect. Has almost the same features as the Kalman filters, but handle nonlinear dynamics and measurement model, still requires an additive Gaussian noise model.
Functionality
- Filtering
- Smoothing
- Parameter estimation using ML or PMMH (Particle Marginal Metropolis Hastings)
Usage example
This example demonstrates how we set up the filters, both PF and KF, for a simple linear system.
Particle filter
Defining a particle filter is straightforward, one must define the distribution of the noise df
in the dynamics function, dynamics(x,u)
and the noise distribution dg
in the measurement function measurement(x)
. The distribution of the initial state d0
must also be provided. An example for a linear Gaussian system is given below.
using LowLevelParticleFilters, LinearAlgebra, StaticArrays, Distributions, Plots
Define problem
n = 2 # Dimension of state
m = 2 # Dimension of input
p = 2 # Dimension of measurements
N = 500 # Number of particles
const dg = MvNormal(p,1.0) # Measurement noise Distribution
const df = MvNormal(n,1.0) # Dynamics noise Distribution
const d0 = MvNormal(randn(n),2.0) # Initial state Distribution
Define random linear state-space system
Tr = randn(n,n)
const A = SMatrix{n,n}(Tr*diagm(0=>LinRange(0.5,0.95,n))/Tr)
const B = @SMatrix randn(n,m)
const C = @SMatrix randn(p,n)
The following two functions are required by the filter
dynamics(x,u) = A*x .+ B*u
measurement(x) = C*x
vecvec_to_mat(x) = copy(reduce(hcat, x)') # Helper function
We are now ready to define and use a filter
pf = ParticleFilter(N, dynamics, measurement, df, dg, d0)
xs,u,y = simulate(pf,200,df) # We can simulate the model that the pf represents
pf(u[1], y[1]) # Perform one filtering step using input u and measurement y
particles(pf) # Query the filter for particles, try weights(pf) or expweights(pf) as well
x̂ = weigthed_mean(pf) # using the current state
If you want to perform filtering using vectors of inputs and measurements, try any of the functions
x,w,we,ll = forward_trajectory(pf, u, y) # Filter whole vectors of signals
x̂,ll = mean_trajectory(pf, u, y)
trajectorydensity(pf,x,w,y,xreal=xs)
If MonteCarloMeasurements.jl is loaded, you may transform the output particles to Matrix{MonteCarloMeasurements.Particles}
with the layout T × n_states
using Particles(x,we)
. Internally, the particles are then resampled such that they all have unit weight. This is conventient for making use of the plotting facilities of MonteCarloMeasurements.jl.
For a full usage example, see the benchmark section below or example_lineargaussian.jl
Smoothing
We also provide a particle smoother, based on forward filtering, backward simulation (FFBS)
N = 2000 # Number of particles
T = 200 # Number of time steps
M = 100 # Number of smoothed backwards trajectories
pf = ParticleFilter(N, dynamics, measurement, df, dg, d0)
du = MvNormal(2,1) # Control input distribution
x,u,y = simulate(pf,T,du) # Simulate trajectory using the model in the filter
tosvec(y) = reinterpret(SVector{length(y[1]),Float64}, reduce(hcat,y))[:] |> copy
x,u,y = tosvec.((x,u,y))
xb,ll = smooth(pf, M, u, y) # Sample smooting particles
xbm = smoothed_mean(xb) # Calculate the mean of smoothing trajectories
xbc = smoothed_cov(xb) # And covariance
xbt = smoothed_trajs(xb) # Get smoothing trajectories
xbs = [diag(xbc) for xbc in xbc] |> vecvec_to_mat .|> sqrt
plot(xbm', ribbon=2xbs, lab="PF smooth")
plot!(vecvec_to_mat(x), l=:dash, lab="True")
We can plot the particles themselves as well
plot(vecvec_to_mat(x), l=(4,), layout=(2,1), show=false)
scatter!(xbt[1,:,:]', subplot=1, show=false, m=(1,:black, 0.5), lab="")
scatter!(xbt[2,:,:]', subplot=2, m=(1,:black, 0.5), lab="")
Kalman filter
A Kalman filter is easily created using the constructor. Many of the functions defined for particle filters, are defined also for Kalman filters, e.g.:
eye(n) = Matrix{Float64}(I,n,n)
kf = KalmanFilter(A, B, C, 0, eye(n), eye(p), MvNormal([1.,1.]))
ukf = UnscentedKalmanFilter(dynamics, measurement, eye(n), eye(p), MvNormal([1.,1.]))
xf,xt,R,Rt,ll = forward_trajectory(kf, u, y) # filtered, prediction, pred cov, filter cov, loglik
xT,R,lls = smooth(kf, u, y) # Smoothed state, smoothed cov, loglik
It can also be called in a loop like the pf
above
for t = 1:T
kf(u,y) # Performs both correct and predict!!
# alternatively
ll += correct!(kf, y, t) # Returns loglik
x = state(kf)
R = covariance(kf)
predict!(kf, u, t)
end
Troubleshooting
Tuning a particle filter can be quite the challenge. To assist with this, we provide som visualization tools
debugplot(pf,u[1:30],y[1:30], runall=true, xreal=x[1:30])
Time Surviving Effective nbr of particles
--------------------------------------------------------------
t: 1 1.000 1000.0
t: 2 1.000 551.0
t: 3 1.000 453.0
t: 4 1.000 384.3
t: 5 1.000 340.9
t: 6 1.000 310.5
t: 7 1.000 280.0
t: 8 1.000 265.9
The plot displays all states and all measurements. The heatmap in the background represents the weighted particle distributions per time step. For the measurement sequences, the heatmap represent the distibutions of predicted measurements. The blue dots corresponds to measured values. In this case, we simulated the data and we had access to states as well, if we do not have that, just omit xreal
.
You can also manually step through the time-series using
commandplot(pf,u,y; kwargs...)
For options to the debug plots, see?pplot
.
Parameter estimation
We provide som basic functionality for maximum likelihood estimation and MAP estimation
ML estimation
Plot likelihood as function of the variance of the dynamics noise
svec = exp10.(LinRange(-1.5,1.5,60))
llspf = map(svec) do s
df = MvNormal(n,s)
pfs = ParticleFilter(2000, dynamics, measurement, df, dg, d0)
loglik(pfs,u,y)
end
plot( svec, llspf,
xscale = :log10,
title = "Log-likelihood",
xlabel = "Dynamics noise standard deviation",
lab = "PF",
)
vline!([svec[findmax(llspf)[2]]], l=(:dash,:blue), primary=false)
We can do the same with a Kalman filter
eye(n) = Matrix{Float64}(I,n,n)
llskf = map(svec) do s
kfs = KalmanFilter(A, B, C, 0, s^2*eye(n), eye(p), d0)
loglik(kfs,u,y)
end
plot!(svec, llskf, yscale=:identity, xscale=:log10, lab="Kalman", c=:red)
vline!([svec[findmax(llskf)[2]]], l=(:dash,:red), primary=false)
as we can see, the result is quite noisy due to the stochastic nature of particle filtering.
Smoothing using KF
kf = KalmanFilter(A, B, C, 0, eye(n), eye(p), MvNormal(2,1))
xf,xh,R,Rt,ll = forward_trajectory(kf, u, y) # filtered, prediction, pred cov, filter cov, loglik
xT,R,lls = smooth(kf, u, y) # Smoothed state, smoothed cov, loglik
Plot and compare PF and KF
plot(vecvec_to_mat(xT), lab="Kalman smooth", layout=2)
plot!(xbm', lab="pf smooth")
plot!(vecvec_to_mat(x), lab="true")
MAP estiamtion
To solve a MAP estimation problem, we need to define a function that takes a parameter vector and returns a particle filter
filter_from_parameters(θ, pf = nothing) = ParticleFilter(
N,
dynamics,
measurement,
MvNormal(n, exp(θ[1])),
MvNormal(p, exp(θ[2])),
d0,
)
The call to exp
on the parameters is so that we can define log-normal priors
priors = [Normal(0,2),Normal(0,2)]
Now we call the function log_likelihood_fun
that returns a function to be minimized
ll = log_likelihood_fun(filter_from_parameters,priors,u,y)
Since this is a low-dimensional problem, we can plot the LL on a 2d-grid
function meshgrid(a,b)
grid_a = [i for i in a, j in b]
grid_b = [j for i in a, j in b]
grid_a, grid_b
end
Nv = 20
v = LinRange(-0.7,1,Nv)
llxy = (x,y) -> ll([x;y])
VGx, VGy = meshgrid(v,v)
VGz = llxy.(VGx, VGy)
heatmap(
VGz,
xticks = (1:Nv, round.(v, digits = 2)),
yticks = (1:Nv, round.(v, digits = 2)),
xlabel = "sigma v",
ylabel = "sigma w",
) # Yes, labels are reversed
Something seems to be off with this figure as the hottest spot is not really where we would expect it
Optimization of the log likelihood can be done by, e.g., global/black box methods, see BlackBoxOptim.jl. Standard tricks apply, such as performing the parameter search in log-space etc.
Bayesian inference using PMMH
This is pretty cool. We procede like we did for MAP above, but when calling the function metropolis
, we will get the entire posterior distribution of the parameter vector, for the small cost of a massive increase in computational cost.
N = 1000
filter_from_parameters(θ, pf = nothing) = AuxiliaryParticleFilter(
N,
dynamics,
measurement,
MvNormal(n, exp(θ[1])),
MvNormal(p, exp(θ[2])),
d0,
)
The call to exp
on the parameters is so that we can define log-normal priors
priors = [Normal(0,2),Normal(0,2)]
ll = log_likelihood_fun(filter_from_parameters,priors,u,y)
θ₀ = log.([1.,1.]) # Starting point
We also need to define a function that suggests a new point from the "proposal distribution". This can be pretty much anything, but it has to be symmetric since I was lazy and simplified an equation.
draw = θ -> θ .+ rand(MvNormal(0.05ones(2)))
burnin = 200
@info "Starting Metropolis algorithm"
@time theta, lls = metropolis(ll, 1200, θ₀, draw) # Run PMMH for 1200 iterations, takes about half a minute on my laptop
thetam = reduce(hcat, theta)'[burnin+1:end,:] # Build a matrix of the output (was vecofvec)
histogram(exp.(thetam), layout=(3,1)); plot!(lls[burnin+1:end], subplot=3) # Visualize
If you are lucky, you can run the above threaded as well. I tried my best to make particle fitlers thread safe with their own rngs etc., but your milage may vary.
@time thetalls = LowLevelParticleFilters.metropolis_threaded(burnin, ll, 500, θ₀, draw)
histogram(exp.(thetalls[:,1:2]), layout=3)
plot!(thetalls[:,3], subplot=3)
AdvancedParticleFilter
The AdvancedParticleFilter
type requires you to implement the same functions as the regular ParticleFilter
, but in this case you also need to handle sampling from the noise distributions yourself.
The function dynamics
must have a method signature like below. It must provide one method that accepts state vector, control vector, time and noise::Bool
that indicates whether or not to add noise to the state. If noise should be added, this should be done inside dynamics
An example is given below
using Random
const rng = Random.MersenneTwister()
function dynamics(x,u,t,noise=false) # It's important that this defaults to false
x = A*x .+ B*u # A simple dynamics model
if noise
x += rand(rng, df) # it's faster to supply your own rng
end
x
end
The measurement_likelihood
function must have a method accepting state, measurement and time, and returning the log-likelihood of the measurement given the state, a simple example below:
function measurement_likelihood(x,y,t)
logpdf(dg, C*x-y) # A simple linear measurement model with normal additive noise
end
This gives you very high flexibility. The noise model in either function can, for instance, be a function of the state, something that is not possible for the simple ParticleFilter
To be able to simulate the AdvancedParticleFilter
like we did with the simple filter above, the measurement
method with the signature measurement(x,t,noise=false)
must be available and return a sample measurement given state (and possibly time). For our example measurement model above, this would look like this
measurement(x,t,noise=false) = C*x + noise*rand(rng, dg)
We now create the AdvancedParticleFilter
and use it in the same way as the other filters:
apf = AdvancedParticleFilter(N, dynamics, measurement, measurement_likelihood, df, d0)
x,w,we,ll = forward_trajectory(apf, u, y)
trajectorydensity(apf, x, we, y, xreal=xs)
We can even use this type as an AuxiliaryParticleFilter
apfa = AuxiliaryParticleFilter(apf)
x,w,we,ll = forward_trajectory(apfa, u, y)
trajectorydensity(apfa, x, we, y, xreal=xs)
dimensiondensity(apfa, x, we, y, 1, xreal=xs) # Same as above, but only plots a single dimension
High performance Distributions
When using LowLevelParticleFilters
, a number of methods related to distributions are defined for static arrays, making logpdf
etc. faster. We also provide a new kind of distribution: TupleProduct <: MultivariateDistribution
that behaves similarly to the Product
distribution. The TupleProduct
however stores the individual distributions in a tuple, has compile-time known length and supports Mixed <: ValueSupport
, meaning that it can be a product of both Continuous
and Discrete
dimensions, somthing not supported by the standard Product
. Example
using BenchmarkTools, LowLevelParticleFilters, Distributions
dt = TupleProduct((Normal(0,2), Normal(0,2), Binomial())) # Mixed value support
A small benchmark
sv = @SVector randn(2)
d = Product([Normal(0,2), Normal(0,2)])
dt = TupleProduct((Normal(0,2), Normal(0,2)))
dm = MvNormal(2, 2)
@btime logpdf($d,$(Vector(sv))) # 32.449 ns (1 allocation: 32 bytes)
@btime logpdf($dt,$(Vector(sv))) # 21.141 ns (0 allocations: 0 bytes)
@btime logpdf($dm,$(Vector(sv))) # 48.745 ns (1 allocation: 96 bytes)
@btime logpdf($d,$sv) # 22.651 ns (0 allocations: 0 bytes)
@btime logpdf($dt,$sv) # 0.021 ns (0 allocations: 0 bytes)
@btime logpdf($dm,$sv) # 0.021 ns (0 allocations: 0 bytes)
Without loading LowLevelParticleFilters
, the timing for the native distributions are the following
@btime logpdf($d,$sv) # 32.621 ns (1 allocation: 32 bytes)
@btime logpdf($dm,$sv) # 46.415 ns (1 allocation: 96 bytes)
Benchmark test
To see how the performance varies with the number of particles, we simulate several times. The following code simulates the system and performs filtering using the simulated measuerments. We do this for varying number of time steps and varying number of particles.
using Random
const rng = Random.MersenneTwister()
function run_test()
particle_count = [10, 20, 50, 100, 200, 500, 1000]
time_steps = [20, 100, 200]
RMSE = zeros(length(particle_count),length(time_steps)) # Store the RMS errors
propagated_particles = 0
t = @elapsed for (Ti,T) = enumerate(time_steps)
for (Ni,N) = enumerate(particle_count)
montecarlo_runs = 2*maximum(particle_count)*maximum(time_steps) ÷ T ÷ N
E = sum(1:montecarlo_runs) do mc_run
pf = ParticleFilter(N, dynamics, measurement, df, dg, d0) # Create filter
u = @SVector randn(2)
x = SVector{2,Float64}(rand(rng, d0))
y = SVector{2,Float64}(sample_measurement(pf,x,1))
error = 0.0
@inbounds for t = 1:T-1
pf(u, y) # Update the particle filter
x = dynamics(x,u) + SVector{2,Float64}(rand(rng, df)) # Simulate the true dynamics and add some noise
y = SVector{2,Float64}(sample_measurement(pf,x,t)) # Simulate a measuerment
u = @SVector randn(2) # draw a random control input
error += sum(abs2,x-weigthed_mean(pf))
end # t
√(error/T)
end # MC
RMSE[Ni,Ti] = E/montecarlo_runs
propagated_particles += montecarlo_runs*N*T
@show N
end # N
@show T
end # T
println("Propagated $propagated_particles particles in $t seconds for an average of $(propagated_particles/t/1000) particles per millisecond")
return RMSE
end
@time RMSE = run_test()
Propagated 8400000 particles in 2.193401766 seconds for an average of 3829.6677472448064 particles per millisecond
We then plot the results
time_steps = [20, 100, 200]
particle_count = [10, 20, 50, 100, 200, 500, 1000]
nT = length(time_steps)
leg = reshape(["$(time_steps[i]) time steps" for i = 1:nT], 1,:)
plot(particle_count,RMSE,xscale=:log10, ylabel="RMS errors", xlabel=" Number of particles", lab=leg)
gui()
This page was generated using Literate.jl.