# UnscentedKalmanFilter

## Introduction and Overview

This implements the unscented Kalman filter.

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.

class filterpy.kalman.UnscentedKalmanFilter(dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None)

  Implements the Scaled Unscented Kalman filter (UKF) as defined by Simon Julier in [1], using the formulation provided by Wan and Merle in [2]. This filter scales the sigma points to avoid strong nonlinearities.

  Parameters:
    dim_x int

      Number of state variables for the filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4.

    dim_z int

      Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2.

      This is for convience, so everything is sized correctly on creation. If you are using multiple sensors the size of z can change based on the sensor. Just provide the appropriate hx function

    dt float

      Time between steps in seconds.

    hx function(x)

      Measurement function. Converts state vector x into a measurement vector of shape (dim_z).

    fx function(x,dt)

      function that returns the state x transformed by the state transistion function. dt is the time step in seconds.

    points class

      Class which computes the sigma points and weights for a UKF algorithm. You can vary the UKF implementation by changing this class. For example, MerweScaledSigmaPoints implements the alpha, beta, kappa parameterization of Van der Merwe, and JulierSigmaPoints implements Julier’s original kappa parameterization. See either of those for the required signature of this class if you want to implement your own.

    sqrt_fn callable(ndarray), default=None (implies scipy.linalg.cholesky)

      Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe’s dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you.

      If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing as far as this class is concerned.

    x_mean_fn callable (sigma_points, weights), optional

      Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed.

          def state_mean(sigmas, Wm):
              x = np.zeros(3)
              sum_sin, sum_cos = 0., 0.

              for i in range(len(sigmas)):
                  s = sigmas[i]
                  x[0] += s[0] * Wm[i]
                  x[1] += s[1] * Wm[i]
                  sum_sin += sin(s[2])*Wm[i]
                  sum_cos += cos(s[2])*Wm[i]
              x[2] = atan2(sum_sin, sum_cos)
              return x

    z_mean_fn callable (sigma_points, weights), optional

      Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx().

    residual_x callable (x, y), optional
    residual_z callable (x, y), optional

      Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. One is for the state variable, the other is for the measurement state.

          def residual(a, b):
              y = a[0] - b[0]
              if y > np.pi:
                  y -= 2*np.pi
              if y < -np.pi:
                  y = 2*np.pi
              return y

  Attributes:
    x numpy.array(dim_x)

      state estimate vector

    P numpy.array(dim_x, dim_x)

      covariance estimate matrix

    x_prior numpy.array(dim_x)

      Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only.

    P_prior numpy.array(dim_x, dim_x)

      Prior (predicted) state covariance matrix. Read Only.

    x_post numpy.array(dim_x)

      Posterior (updated) state estimate. Read Only.

    P_post numpy.array(dim_x, dim_x)

      Posterior (updated) state covariance matrix. Read Only.

    z ndarray

      Last measurement used in update(). Read only.

    R numpy.array(dim_z, dim_z)

      measurement noise matrix

    Q numpy.array(dim_x, dim_x)

      process noise matrix

    K numpy.array

      Kalman gain

    y numpy.array

      innovation residual

    `log_likelihood` scalar

      log-likelihood of the last measurement.

    `likelihood` float

      Computed from the log-likelihood.

    `mahalanobis` float

      “

    inv function, default numpy.linalg.inv

      If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead:

          kf.inv = np.linalg.pinv

  References

  [1]

  Julier, Simon J. “The scaled unscented transformation,” American Control Converence, 2002, pp 4555-4559, vol 6.

  Online copy: https://www.cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF

  [2]

  E. A. Wan and R. Van der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000.

  Online Copy: https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf

  [3]

  S. Julier, J. Uhlmann, and H. Durrant-Whyte. “A new method for the nonlinear transformation of means and covariances in filters and estimators,” IEEE Transactions on Automatic Control, 45(3), pp. 477-482 (March 2000).

  [4]

  E. A. Wan and R. Van der Merwe, “The Unscented Kalman filter for Nonlinear Estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000.

  https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf

  [5]

  Wan, Merle “The Unscented Kalman Filter,” chapter in Kalman Filtering and Neural Networks, John Wiley & Sons, Inc., 2001.

  [6]

  R. Van der Merwe “Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models” (Doctoral dissertation)

  Examples

  Simple example of a linear order 1 kinematic filter in 2D. There is no need to use a UKF for this example, but it is easy to read.

      >>> def fx(x, dt):
      >>>     # state transition function - predict next state based
      >>>     # on constant velocity model x = vt + x_0
      >>>     F = np.array([[1, dt, 0, 0],
      >>>                   [0, 1, 0, 0],
      >>>                   [0, 0, 1, dt],
      >>>                   [0, 0, 0, 1]], dtype=float)
      >>>     return np.dot(F, x)
      >>>
      >>> def hx(x):
      >>>    # measurement function - convert state into a measurement
      >>>    # where measurements are [x_pos, y_pos]
      >>>    return np.array([x[0], x[2]])
      >>>
      >>> dt = 0.1
      >>> # create sigma points to use in the filter. This is standard for Gaussian processes
      >>> points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1)
      >>>
      >>> kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
      >>> kf.x = np.array([-1., 1., -1., 1]) # initial state
      >>> kf.P *= 0.2 # initial uncertainty
      >>> z_std = 0.1
      >>> kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
      >>> kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2)
      >>>
      >>> zs = [[i+randn()*z_std, i+randn()*z_std] for i in range(50)] # measurements
      >>> for z in zs:
      >>>     kf.predict()
      >>>     kf.update(z)
      >>>     print(kf.x, 'log-likelihood', kf.log_likelihood)

  For in depth explanations see my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

  Also see the filterpy/kalman/tests subdirectory for test code that may be illuminating.

  __init__(dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None)

    Create a Kalman filter. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter.

  predict(dt=None, UT=None, fx=None, **fx_args)

    Performs the predict step of the UKF. On return, self.x and self.P contain the predicted state (x) and covariance (P). ‘

    Important: this MUST be called before update() is called for the first time.

    Parameters:
      dt double, optional

        If specified, the time step to be used for this prediction. self._dt is used if this is not provided.

      fx callable f(x, **fx_args), optional

        State transition function. If not provided, the default function passed in during construction will be used.

      UT function(sigmas, Wm, Wc, noise_cov), optional

        Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform.

      **fx_args keyword arguments

        optional keyword arguments to be passed into f(x).

  update(z, R=None, UT=None, hx=None, **hx_args)

    Update the UKF with the given measurements. On return, self.x and self.P contain the new mean and covariance of the filter.

    Parameters:
      z numpy.array of shape (dim_z)

        measurement vector

      R numpy.array((dim_z, dim_z)), optional

        Measurement noise. If provided, overrides self.R for this function call.

      UT function(sigmas, Wm, Wc, noise_cov), optional

        Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform.

      **hx_args keyword argument

        arguments to be passed into h(x) after x -> h(x, **hx_args)

  cross_variance(x, z, sigmas_f, sigmas_h)

    Compute cross variance of the state x and measurement z.

  compute_process_sigmas(dt, fx=None, **fx_args)

    computes the values of sigmas_f. Normally a user would not call this, but it is useful if you need to call update more than once between calls to predict (to update for multiple simultaneous measurements), so the sigmas correctly reflect the updated state x, P.

  batch_filter(zs, Rs=None, dts=None, UT=None, saver=None)

    Performs the UKF filter over the list of measurement in zs.

    Parameters:
      zs list-like

        list of measurements at each time step self._dt Missing measurements must be represented by ‘None’.

      Rs None, np.array or list-like, default=None

        optional list of values to use for the measurement error covariance R.

        If Rs is None then self.R is used for all epochs.

        If it is a list of matrices or a 3D array where len(Rs) == len(zs), then it is treated as a list of R values, one per epoch. This allows you to have varying R per epoch.

      dts None, scalar or list-like, default=None

        optional value or list of delta time to be passed into predict.

        If dtss is None then self.dt is used for all epochs.

        If it is a list where len(dts) == len(zs), then it is treated as a list of dt values, one per epoch. This allows you to have varying epoch durations.

      UT function(sigmas, Wm, Wc, noise_cov), optional

        Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform.

      saver filterpy.common.Saver, optional

        filterpy.common.Saver object. If provided, saver.save() will be called after every epoch

    Returns:
      means: ndarray((n,dim_x,1))

        array of the state for each time step after the update. Each entry is an np.array. In other words means[k,:] is the state at step k.

      covariance: ndarray((n,dim_x,dim_x))

        array of the covariances for each time step after the update. In other words covariance[k,:,:] is the covariance at step k.

    Examples

        # this example demonstrates tracking a measurement where the time
        # between measurement varies, as stored in dts The output is then smoothed
        # with an RTS smoother.

        zs = [t + random.randn()*4 for t in range (40)]

        (mu, cov, _, _) = ukf.batch_filter(zs, dts=dts)
        (xs, Ps, Ks) = ukf.rts_smoother(mu, cov)

  rts_smoother(Xs, Ps, Qs=None, dts=None, UT=None)

    Runs the Rauch-Tung-Striebal Kalman smoother on a set of means and covariances computed by the UKF. The usual input would come from the output of batch_filter().

    Parameters:
      Xs numpy.array

        array of the means (state variable x) of the output of a Kalman filter.

      Ps numpy.array

        array of the covariances of the output of a kalman filter.

      Qs: list-like collection of numpy.array, optional

        Process noise of the Kalman filter at each time step. Optional, if not provided the filter’s self.Q will be used

      dt optional, float or array-like of float

        If provided, specifies the time step of each step of the filter. If float, then the same time step is used for all steps. If an array, then each element k contains the time at step k. Units are seconds.

      UT function(sigmas, Wm, Wc, noise_cov), optional

        Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform.

    Returns:
      x numpy.ndarray

        smoothed means

      P numpy.ndarray

        smoothed state covariances

      K numpy.ndarray

        smoother gain at each step

    Examples

        zs = [t + random.randn()*4 for t in range (40)]

        (mu, cov, _, _) = kalman.batch_filter(zs)
        (x, P, K) = rts_smoother(mu, cov, fk.F, fk.Q)

  property log_likelihood

    log-likelihood of the last measurement.

  property likelihood

    Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min.

  property mahalanobis

    ” Mahalanobis distance of measurement. E.g. 3 means measurement was 3 standard deviations away from the predicted value.

    Returns:
      mahalanobis float

class filterpy.kalman.MerweScaledSigmaPoints(n, alpha, beta, kappa, sqrt_method=None, subtract=None)

  Generates sigma points and weights according to Van der Merwe’s 2004 dissertation[1] for the UnscentedKalmanFilter class.. It parametizes the sigma points using alpha, beta, kappa terms, and is the version seen in most publications.

  Unless you know better, this should be your default choice.

  Parameters:
    n int

      Dimensionality of the state. 2n+1 weights will be generated.

    alpha float

      Determins the spread of the sigma points around the mean. Usually a small positive value (1e-3) according to [3].

    beta float

      Incorporates prior knowledge of the distribution of the mean. For Gaussian x beta=2 is optimal, according to [3].

    kappa float, default=0.0

      Secondary scaling parameter usually set to 0 according to [4], or to 3-n according to [5].

    sqrt_method function(ndarray), default=scipy.linalg.cholesky

      Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe’s dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you.

      If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing.

    subtract callable (x, y), optional

      Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars.

  Attributes:
    Wm np.array

      weight for each sigma point for the mean

    Wc np.array

      weight for each sigma point for the covariance

  References

  [1]

  R. Van der Merwe “Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models” (Doctoral dissertation)

  Examples

  See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

  __init__(n, alpha, beta, kappa, sqrt_method=None, subtract=None)
  num_sigmas()

    Number of sigma points for each variable in the state x

  sigma_points(x, P)

    Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights.

    Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I

    Parameters:
      x An array-like object of the means of length n

        Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2])

      P scalar, or np.array

        Covariance of the filter. If scalar, is treated as eye(n)*P.

    Returns:
      sigmas np.array, of size (n, 2n+1)

        Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space.

        Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}

class filterpy.kalman.JulierSigmaPoints(n, kappa=0.0, sqrt_method=None, subtract=None)

  Generates sigma points and weights according to Simon J. Julier and Jeffery K. Uhlmann’s original paper[1]. It parametizes the sigma points using kappa.

  Parameters:
    n int

      Dimensionality of the state. 2n+1 weights will be generated.

    kappa float, default=0.

      Scaling factor that can reduce high order errors. kappa=0 gives the standard unscented filter. According to [Julier], if you set kappa to 3-dim_x for a Gaussian x you will minimize the fourth order errors in x and P.

    sqrt_method function(ndarray), default=scipy.linalg.cholesky

      Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe’s dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you.

      If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing.

    subtract callable (x, y), optional

      Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y

  Attributes:
    Wm np.array

      weight for each sigma point for the mean

    Wc np.array

      weight for each sigma point for the covariance

  References

  [1]

  Julier, Simon J.; Uhlmann, Jeffrey “A New Extension of the Kalman Filter to Nonlinear Systems”. Proc. SPIE 3068, Signal Processing, Sensor Fusion, and Target Recognition VI, 182 (July 28, 1997)

  __init__(n, kappa=0.0, sqrt_method=None, subtract=None)
  num_sigmas()

    Number of sigma points for each variable in the state x

  sigma_points(x, P)

    Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. kappa is an arbitrary constant. Returns sigma points.

    Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I

    Parameters:
      x array-like object of the means of length n

        Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2])

      P scalar, or np.array

        Covariance of the filter. If scalar, is treated as eye(n)*P.

      kappa float

        Scaling factor.

    Returns:
      sigmas np.array, of size (n, 2n+1)

        2D array of sigma points \(\chi\). Each column contains all of the sigmas for one dimension in the problem space. They are ordered as:

        \begin{eqnarray} \chi[0] = &x \\ \chi[1..n] = &x + [\sqrt{(n+\kappa)P}]_k \\ \chi[n+1..2n] = &x - [\sqrt{(n+\kappa)P}]_k \end{eqnarray}

class filterpy.kalman.SimplexSigmaPoints(n, alpha=1, sqrt_method=None, subtract=None)

  Generates sigma points and weights according to the simplex method presented in [1].

  Parameters:
    n int

      Dimensionality of the state. n+1 weights will be generated.

    sqrt_method function(ndarray), default=scipy.linalg.cholesky

      Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm

      If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing.

    subtract callable (x, y), optional

      Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars.

  Attributes:
    Wm np.array

      weight for each sigma point for the mean

    Wc np.array

      weight for each sigma point for the covariance

  References

  [1]

  Phillippe Moireau and Dominique Chapelle “Reduced-Order Unscented Kalman Filtering with Application to Parameter Identification in Large-Dimensional Systems” DOI: 10.1051/cocv/2010006

  __init__(n, alpha=1, sqrt_method=None, subtract=None)
  num_sigmas()

    Number of sigma points for each variable in the state x

  sigma_points(x, P)

    Computes the implex sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights.

    Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I

    Parameters:
      x An array-like object of the means of length n

        Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2])

      P scalar, or np.array

        Covariance of the filter. If scalar, is treated as eye(n)*P.

    Returns:
      sigmas np.array, of size (n, n+1)

        Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space.

        Ordered by Xi_0, Xi_{1..n}