## LowLevelParticleFilters.jl

Simple particle/kalman filtering, smoothing and parameter estimation
Author baggepinnen
Popularity
33 Stars
Updated Last
2 Years Ago
Started In
February 2018

# 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 to `ParticleFilter`, 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, y) # 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),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)]], 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)]], 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(θ)),
MvNormal(p, exp(θ)),
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(θ)),
MvNormal(p, exp(θ)),
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)```

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()``` 