Home

Awesome

pfilter

Basic Python particle filter. Plain SIR filtering, with various resampling algorithms. Written to be simple and clear; not necessarily most efficient or most flexible implementation. Depends on NumPy only.

Uses

This repo is useful for understanding how a particle filter works, or a quick way to develop a custom filter of your own from a relatively simple codebase.

Alternatives

There are more mature and sophisticated packages for probabilistic filtering in Python (especially for Kalman filtering) if you want an off-the-shelf solution:

Particle filtering

Kalman filtering

Installation

Available via PyPI:

pip install pfilter

Or install the git version:

pip install git+https://github.com/johnhw/pfilter.git

Usage

Create a ParticleFilter object, then call update(observation) with an observation array to update the state of the particle filter.

Calling update() without an observation will update the model without any data, i.e. perform a prediction step only.

Model

Particles are represented as an (n,d) matrix of states, one state per row. Observations are generated from this matrix into an (n,h) matrix of hypothesized observations via the observation function.

Functions

You need to specify at the minimum:


Typically, you would also specify:


You might also specify:

Missing observations

If you want to be able to deal with partial missing values in the observations, the weight function should support masked arrays. The squared_error(a,b) function in pfilter.py does this, for example.

Passing values to functions

Sometimes it is useful to pass inputs to callback functions like dynamics_fn(x) at each time step. You can do this by giving keyword arguments to update().

If you call pf.update(y, t=5) all of the functions dynamics_fn, weight_fn, noise_fn, internal_weight_fn, observe_fn will receive the keyword argument t=5. ALl kwargs are forwarded to these calls. You can just ignore them if not used (e.g. define dynamics_fn = lambda x, **kwargs: real_dynamics(x)) but this can be useful for propagating inputs that are neither internal states nor observed states to the filter. If no kwargs are given to update, then no extra arguments are passed to any of callbacks.

Attributes

The ParticleFilter object will have the following useful attributes after updating:

In equations

Example

For example, assuming we observe 32x32 images and want to track a moving circle. Assume the internal state we are estimating is the 4D vector (x, y, dx, dy), with 200 particles

        from pfilter import ParticleFilter, gaussian_noise, squared_error, independent_sample
        columns = ["x", "y", "radius", "dx", "dy"]
        from scipy.stats import norm, gamma, uniform 
        
        # prior sampling function for each variable
        # (assumes x and y are coordinates in the range 0-32)    
        prior_fn = independent_sample([uniform(loc=0, scale=32).rvs, 
                    uniform(loc=0, scale=32).rvs, 
                    gamma(a=2,loc=0,scale=10).rvs,
                    norm(loc=0, scale=0.5).rvs,
                    norm(loc=0, scale=0.5).rvs])
                                    
        # very simple linear dynamics: x += dx
        def velocity(x):
            xp = np.array(x)
            xp[0:2] += xp[3:5]        
        return xp
        
        # create the filter
        pf = pfilter.ParticleFilter(
                        prior_fn=prior_fn, 
                        observe_fn=blob,
                        n_particles=200,
                        dynamics_fn=velocity,
                        noise_fn=lambda x: 
                                    gaussian_noise(x, sigmas=[0.2, 0.2, 0.1, 0.05, 0.05]),
                        weight_fn=lambda x,y:squared_error(x, y, sigma=2),
                        resample_proportion=0.1,
                        column_names = columns)
                        
        # assuming image of the same dimensions/type as blob will produce
        pf.update(image) 

See the notebook examples/example_filter.py for a working example using skimage and OpenCV which tracks a moving white circle.


<!-- \begin{align*} d & \in \mathbb{Z}^+ & \text{state dimension} \\ h & \in \mathbb{Z}^+& \text{observation dimension} \\ n & \in \mathbb{Z}^+& \text{number of particles} \\ {\bf x}^i &\in \mathbb{R}^d & \text{state vectors}\\ {\bf y}^i &\in \mathbb{R}^h & \text{observation vectors}\\ {\bf x}^{i}(0) & \sim \pi({\bf x}) & \text{prior}\\ {\bf x}^i(t) & = g({\bf x}^i(t-1)) + \epsilon(t) & \text{dynamics}\\ {\bf y}^i(t) & = f({\bf x}^i(t))\ & \text{observation}\\ w_i & = k\left({\bf y}^{i}(t), {\bf y}'(t)\right)v({\bf x}^{i}(t)) & \text{weight}\\ w'_i & = \frac{w_i}{\sum_j w_j} & \text{normalisation}\\ k\left({\bf y}, {\bf y'}\right) & \ (\mathbb{R}^h, \mathbb{R}^h) \rightarrow \mathbb{R^+} & \text{weight function} \\ v({\bf x}) &\ \mathbb{R}^d\rightarrow\mathbb{R}^+ & \text{internal weight function}\\ g(\bf{x}) & \ \mathbb{R}^d \rightarrow \mathbb{R}^d & \text{dynamics function} \\ f(\bf{x}) & \ \mathbb{R}^d \rightarrow \mathbb{R}^h & \text{observation function} \\ \end{align*} -->