# 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=())

  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)

  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)

  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)

  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)

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

  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)

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

filterpy.common.runge_kutta4(y, x, dx, f)

  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)

  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

filterpy.common.outer_product_sum(A, B=None)

  Computes the sum of the outer products of the rows in A and B

  > P = Sum {A[i] B[i].T} for i in 0..N

  > Notionally:

  > P = 0 for y in A:

  > > P += np.outer(y, y)

  This is a standard computation for sigma points used in the UKF, ensemble Kalman filter, etc., where A would be the residual of the sigma points and the filter’s state or measurement.

  The computation is vectorized, so it is much faster than the for loop for large A.

  Parameters:
    A np.array, shape (M, N)

      rows of N-vectors to have the outer product summed

    B np.array, shape (M, N)

      rows of N-vectors to have the outer product summed If it is None, it is set to A.

  Returns:
    P np.array, shape(N, N)

      sum of the outer product of the rows of A and B

  Examples

  Here sigmas is of shape (M, N), and x is of shape (N). The two sets of code compute the same thing.

      >>> P = outer_product_sum(sigmas - x)
      >>>
      >>> P = 0
      >>> for s in sigmas:
      >>>     y = s - x
      >>>     P += np.outer(y, y)