common

A collection of functions used throughout FilterPy, and/or functions that you will find useful when you build your filters.

Copyright 2015 Roger R Labbe Jr.

FilterPy library. http://github.com/rlabbe/filterpy

Documentation at: https://filterpy.readthedocs.org

Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

This is licensed under an MIT license. See the readme.MD file for more information.


filterpy.common.Saver(kf, save_current=False, skip_private=False, skip_callable=False, ignore=())[source]

Helper class to save the states of any filter object. Each time you call save() all of the attributes (state, covariances, etc) are appended to lists.

Generally you would do this once per epoch - predict/update.

Then, you can access any of the states by using the [] syntax or by using the . operator.

my_saver = Saver()
... do some filtering

x = my_saver['x']
x = my_save.x

Either returns a list of all of the state x values for the entire filtering process.

If you want to convert all saved lists into numpy arrays, call to_array().

Parameters:
kf : object

any object with a __dict__ attribute, but intended to be one of the filtering classes

save_current : bool, default=True

save the current state of kf when the object is created;

skip_private: bool, default=False

Control skipping any private attribute (anything starting with ‘_’) Turning this on saves memory, but slows down execution a bit.

skip_callable: bool, default=False

Control skipping any attribute which is a method. Turning this on saves memory, but slows down execution a bit.

ignore: (str,) tuple of strings

list of keys to ignore.

Examples

kf = KalmanFilter(...whatever)
# initialize kf here

saver = Saver(kf) # save data for kf filter
for z in zs:
    kf.predict()
    kf.update(z)
    saver.save()

x = np.array(s.x) # get the kf.x state in an np.array
plt.plot(x[:, 0], x[:, 2])

# ... or ...
s.to_array()
plt.plot(s.x[:, 0], s.x[:, 2])

filterpy.common.Q_discrete_white_noise(dim, dt=1.0, var=1.0, block_size=1, order_by_dim=True)[source]

Returns the Q matrix for the Discrete Constant White Noise Model. dim may be either 2, 3, or 4 dt is the time step, and sigma is the variance in the noise.

Q is computed as the G * G^T * variance, where G is the process noise per time step. In other words, G = [[.5dt^2][dt]]^T for the constant velocity model.

Parameters:
dim : int (2, 3, or 4)

dimension for Q, where the final dimension is (dim x dim)

dt : float, default=1.0

time step in whatever units your filter is using for time. i.e. the amount of time between innovations

var : float, default=1.0

variance in the noise

block_size : int >= 1

If your state variable contains more than one dimension, such as a 3d constant velocity model [x x’ y y’ z z’]^T, then Q must be a block diagonal matrix.

order_by_dim : bool, default=True

Defines ordering of variables in the state vector. True orders by keeping all derivatives of each dimensions)

[x x’ x’’ y y’ y’‘]

whereas False interleaves the dimensions

[x y z x’ y’ z’ x’’ y’’ z’‘]

References

Bar-Shalom. “Estimation with Applications To Tracking and Navigation”. John Wiley & Sons, 2001. Page 274.

Examples

>>> # constant velocity model in a 3D world with a 10 Hz update rate
>>> Q_discrete_white_noise(2, dt=0.1, var=1., block_size=3)
array([[0.000025, 0.0005  , 0.      , 0.      , 0.      , 0.      ],
       [0.0005  , 0.01    , 0.      , 0.      , 0.      , 0.      ],
       [0.      , 0.      , 0.000025, 0.0005  , 0.      , 0.      ],
       [0.      , 0.      , 0.0005  , 0.01    , 0.      , 0.      ],
       [0.      , 0.      , 0.      , 0.      , 0.000025, 0.0005  ],
       [0.      , 0.      , 0.      , 0.      , 0.0005  , 0.01    ]])

filterpy.common.Q_continuous_white_noise(dim, dt=1.0, spectral_density=1.0, block_size=1, order_by_dim=True)[source]

Returns the Q matrix for the Discretized Continuous White Noise Model. dim may be either 2, 3, 4, dt is the time step, and sigma is the variance in the noise.

Parameters:
dim : int (2 or 3 or 4)

dimension for Q, where the final dimension is (dim x dim) 2 is constant velocity, 3 is constant acceleration, 4 is constant jerk

dt : float, default=1.0

time step in whatever units your filter is using for time. i.e. the amount of time between innovations

spectral_density : float, default=1.0

spectral density for the continuous process

block_size : int >= 1

If your state variable contains more than one dimension, such as a 3d constant velocity model [x x’ y y’ z z’]^T, then Q must be a block diagonal matrix.

order_by_dim : bool, default=True

Defines ordering of variables in the state vector. True orders by keeping all derivatives of each dimensions)

[x x’ x’’ y y’ y’‘]

whereas False interleaves the dimensions

[x y z x’ y’ z’ x’’ y’’ z’‘]

Examples

>>> # constant velocity model in a 3D world with a 10 Hz update rate
>>> Q_continuous_white_noise(2, dt=0.1, block_size=3)
array([[0.00033333, 0.005     , 0.        , 0.        , 0.        , 0.        ],
       [0.005     , 0.1       , 0.        , 0.        , 0.        , 0.        ],
       [0.        , 0.        , 0.00033333, 0.005     , 0.        , 0.        ],
       [0.        , 0.        , 0.005     , 0.1       , 0.        , 0.        ],
       [0.        , 0.        , 0.        , 0.        , 0.00033333, 0.005     ],
       [0.        , 0.        , 0.        , 0.        , 0.005     , 0.1       ]])

filterpy.common.van_loan_discretization(F, G, dt)[source]

Discretizes a linear differential equation which includes white noise according to the method of C. F. van Loan [1]. Given the continuous model

x’ = Fx + Gu

where u is the unity white noise, we compute and return the sigma and Q_k that discretizes that equation.

References

[1] C. F. van Loan. “Computing Integrals Involving the Matrix Exponential.”
IEEE Trans. Automomatic Control, AC-23 (3): 395-404 (June 1978)
[2] Robert Grover Brown. “Introduction to Random Signals and Applied
Kalman Filtering.” Forth edition. John Wiley & Sons. p. 126-7. (2012)

Examples

Given y’’ + y = 2u(t), we create the continuous state model of

x’ = [ 0 1] * x + [0]*u(t)
[-1 0] [2]

and a time step of 0.1:

>>> F = np.array([[0,1],[-1,0]], dtype=float)
>>> G = np.array([[0.],[2.]])
>>> phi, Q = van_loan_discretization(F, G, 0.1)
>>> phi
array([[ 0.99500417,  0.09983342],
       [-0.09983342,  0.99500417]])
>>> Q
array([[ 0.00133067,  0.01993342],
       [ 0.01993342,  0.39866933]])

(example taken from Brown[2])


filterpy.common.linear_ode_discretation(F, L=None, Q=None, dt=1.0)[source]

filterpy.common.kinematic_kf(dim, order, dt=1.0, dim_z=1, order_by_dim=True)[source]

Returns a KalmanFilter using newtonian kinematics of arbitrary order for any number of dimensions. For example, a constant velocity filter in 3D space would have order 1 dimension 3.

Parameters:
dim : int, >= 1

number of dimensions (2D space would be dim=2)

order : int, >= 0

order of the filter. 2 would be a const acceleration model with a stat

dim_z : int, default 1

size of z vector per dimension dim. Normally should be 1

dt : float, default 1.0

Time step. Used to create the state transition matrix

order_by_dim : bool, default=True

Defines ordering of variables in the state vector. True orders by keeping all derivatives of each dimensions)

[x x’ x’’ y y’ y’‘]

whereas False interleaves the dimensions

[x y z x’ y’ z’ x’’ y’’ z’‘]

Examples

A constant velocity filter in 3D space with delta time = .2 seconds would be created with

>>> kf = kinematic_kf(dim=3, order=1, dt=.2)
>>> kf.F
>>> array([[1. , 0.2, 0. , 0. , 0. , 0. ],
           [0. , 1. , 0. , 0. , 0. , 0. ],
           [0. , 0. , 1. , 0.2, 0. , 0. ],
           [0. , 0. , 0. , 1. , 0. , 0. ],
           [0. , 0. , 0. , 0. , 1. , 0.2],
           [0. , 0. , 0. , 0. , 0. , 1. ]])

which will set the state x to be interpreted as

[x, x’, y, y’, z, z’].T

If you set order_by_dim to False, then x is ordered as

[x y z x’ y’ z’].T

As another example, a 2D constant jerk is created with

>> kinematic_kf(2, 3)

Assumes that the measurement z is position in each dimension. If this is not true you will have to alter the H matrix by hand.

P, Q, R are all set to the Identity matrix.

H is assigned assuming the measurement is position, one per dimension dim.

>>> kf = kinematic_kf(2, 1, dt=3.0)
>>> kf.F
array([[1., 3., 0., 0.],
       [0., 1., 0., 0.],
       [0., 0., 1., 3.],
       [0., 0., 0., 1.]])

filterpy.common.kinematic_state_transition(order, dt)[source]

create a state transition matrix of a given order for a given time step dt.


filterpy.common.runge_kutta4(y, x, dx, f)[source]

computes 4th order Runge-Kutta for dy/dx.

Parameters:
y : scalar

Initial/current value for y

x : scalar

Initial/current value for x

dx : scalar

difference in x (e.g. the time step)

f : ufunc(y,x)

Callable function (y, x) that you supply to compute dy/dx for the specified values.


filterpy.common.inv_diagonal(S)[source]

Computes the inverse of a diagonal NxN np.array S. In general this will be much faster than calling np.linalg.inv().

However, does NOT check if the off diagonal elements are non-zero. So long as S is truly diagonal, the output is identical to np.linalg.inv().

Parameters:
S : np.array

diagonal NxN array to take inverse of

Returns:
S_inv : np.array

inverse of S

Examples

This is meant to be used as a replacement inverse function for the KalmanFilter class when you know the system covariance S is diagonal. It just makes the filter run faster, there is

>>> kf = KalmanFilter(dim_x=3, dim_z=1)
>>> kf.inv = inv_diagonal  # S is 1x1, so safely diagonal