Unscented Kalman Filter

UnscentedKalmanFilter

Introduction and Overview

This implements the unscented Kalman filter.

API Reference

UnscentedKalmanFilter

UnscentedKalmanFilter

Bases: object

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:

Name	Type	Description	Default
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.	required
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	required
hx	function(x, **hx_args)	Measurement function. Converts state vector x into a measurement vector of shape (dim_z).	required
fx	function(x, dt, **fx_args)	function that returns the state x transformed by the state transition function. dt is the time step in seconds.	required
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.	required
sqrt_fn	callable(ndarray)	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.	None (implies scipy.linalg.cholesky)
x_mean_fn	callable(sigma_points, weights)	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. .. code-block:: Python 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	None
z_mean_fn	callable(sigma_points, weights)	Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx().	None
residual_x	callable(x, y)		None
residual_z	callable(x, y)	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. .. code-block:: Python 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	None
state_add		Function that subtracts two state vectors, returning a new state vector. Used during update to compute x + K@y You will have to supply this if your state variable does not suport addition, such as it contains angles.	None

Attributes:

Name	Type	Description
x	array(dim_x)	state estimate vector
P	array(dim_x, dim_x)	covariance estimate matrix
x_prior	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	array(dim_x, dim_x)	Prior (predicted) state covariance matrix. Read Only.
x_post	array(dim_x)	Posterior (updated) state estimate. Read Only.
P_post	array(dim_x, dim_x)	Posterior (updated) state covariance matrix. Read Only.
z	ndarray	Last measurement used in update(). Read only.
R	array(dim_z, dim_z)	measurement noise matrix
Q	array(dim_x, dim_x)	process noise matrix
K	array	Kalman gain
y	array	innovation residual
log_likelihood	scalar	Log likelihood of last measurement update.
likelihood	float	likelihood of last measurment. Read only. 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.
mahalanobis	float	mahalanobis distance of the measurement. Read only.
inv	function, default numpy.linalg.inv	If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead: .. code-block:: Python kf.inv = np.linalg.pinv

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 bayesian_filters/kalman/tests subdirectory for test code that may be illuminating.

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)

Source code in bayesian_filters/kalman/UKF.py

class UnscentedKalmanFilter(object):
    # pylint: disable=too-many-instance-attributes
    # pylint: disable=invalid-name
    r"""
    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





    hx : function(x,**hx_args)
        Measurement function. Converts state vector x into a measurement
        vector of shape (dim_z).

    fx : function(x,dt,**fx_args)
        function that returns the state x transformed by the
        state transition 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.

        .. code-block:: Python

            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.

        .. code-block:: Python

            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

    state_add: callable (x, y), optional, default np.add
        Function that subtracts two state vectors, returning a new
        state vector. Used during update to compute `x + K@y`
        You will have to supply this if your state variable does not
        suport addition, such as it contains angles.

    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 last measurement update.

    likelihood : float
        likelihood of last measurment. Read only.

        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.

    mahalanobis : float
        mahalanobis distance of the measurement. Read only.

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

        .. code-block:: Python

            kf.inv = np.linalg.pinv


    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 bayesian_filters/kalman/tests subdirectory for test code that
    may be illuminating.

    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)
    """

    def __init__(
        self,
        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,
        state_add=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.

        """

        # pylint: disable=too-many-arguments

        self.x = zeros(dim_x)
        self.P = eye(dim_x)
        self.x_prior = np.copy(self.x)
        self.P_prior = np.copy(self.P)
        self.Q = eye(dim_x)
        self.R = eye(dim_z)
        self._dim_x = dim_x
        self._dim_z = dim_z
        self.points_fn = points
        self._dt = dt
        self._num_sigmas = points.num_sigmas()
        self.hx = hx
        self.fx = fx
        self.x_mean = x_mean_fn
        self.z_mean = z_mean_fn

        # Only computed only if requested via property
        self._log_likelihood = log(sys.float_info.min)
        self._likelihood = sys.float_info.min
        self._mahalanobis = None

        if sqrt_fn is None:
            self.msqrt = cholesky
        else:
            self.msqrt = sqrt_fn

        # weights for the means and covariances.
        self.Wm, self.Wc = points.Wm, points.Wc

        if residual_x is None:
            self.residual_x = np.subtract
        else:
            self.residual_x = residual_x

        if residual_z is None:
            self.residual_z = np.subtract
        else:
            self.residual_z = residual_z

        if state_add is None:
            self.state_add = np.add
        else:
            self.state_add = state_add

        # sigma points transformed through f(x) and h(x)
        # variables for efficiency so we don't recreate every update

        self.sigmas_f = zeros((self._num_sigmas, self._dim_x))
        self.sigmas_h = zeros((self._num_sigmas, self._dim_z))

        self.K = np.zeros((dim_x, dim_z))  # Kalman gain
        self.y = np.zeros((dim_z))  # residual
        self.z = np.array([[None] * dim_z]).T  # measurement
        self.S = np.zeros((dim_z, dim_z))  # system uncertainty
        self.SI = np.zeros((dim_z, dim_z))  # inverse system uncertainty

        self.inv = np.linalg.inv

        # these will always be a copy of x,P after predict() is called
        self.x_prior = self.x.copy()
        self.P_prior = self.P.copy()

        # these will always be a copy of x,P after update() is called
        self.x_post = self.x.copy()
        self.P_post = self.P.copy()

    def predict(self, dt=None, UT=None, fx=None, **fx_args):
        r"""
        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, dt, **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).
        """

        if dt is None:
            dt = self._dt

        if UT is None:
            UT = unscented_transform

        # calculate sigma points for given mean and covariance
        self.compute_process_sigmas(dt, fx, **fx_args)

        # and pass sigmas through the unscented transform to compute prior
        self.x, self.P = UT(self.sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x)

        # update sigma points to reflect the new variance of the points
        self.sigmas_f = self.points_fn.sigma_points(self.x, self.P)

        # save prior
        self.x_prior = np.copy(self.x)
        self.P_prior = np.copy(self.P)

    def update(self, 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 : callable h(x, **hx_args), optional
            Measurement function. If not provided, the default
            function passed in during construction will be used.

        **hx_args : keyword argument
            arguments to be passed into h(x) after x -> h(x, **hx_args)
        """

        if z is None:
            self.z = np.array([[None] * self._dim_z]).T
            self.x_post = self.x.copy()
            self.P_post = self.P.copy()
            return

        if hx is None:
            hx = self.hx

        if UT is None:
            UT = unscented_transform

        if R is None:
            R = self.R
        elif isscalar(R):
            R = eye(self._dim_z) * R

        # pass prior sigmas through h(x) to get measurement sigmas
        # the shape of sigmas_h will vary if the shape of z varies, so
        # recreate each time
        sigmas_h = []
        for s in self.sigmas_f:
            sigmas_h.append(hx(s, **hx_args))

        self.sigmas_h = np.atleast_2d(sigmas_h)

        # mean and covariance of prediction passed through unscented transform
        zp, self.S = UT(self.sigmas_h, self.Wm, self.Wc, R, self.z_mean, self.residual_z)
        self.SI = self.inv(self.S)

        # compute cross variance of the state and the measurements
        Pxz = self.cross_variance(self.x, zp, self.sigmas_f, self.sigmas_h)

        self.K = dot(Pxz, self.SI)  # Kalman gain
        self.y = self.residual_z(z, zp)  # residual

        # update Gaussian state estimate (x, P)
        self.x = self.state_add(self.x, dot(self.K, self.y))
        self.P = self.P - dot(self.K, dot(self.S, self.K.T))

        # save measurement and posterior state
        self.z = deepcopy(z)
        self.x_post = self.x.copy()
        self.P_post = self.P.copy()

        # set to None to force recompute
        self._log_likelihood = None
        self._likelihood = None
        self._mahalanobis = None

    def cross_variance(self, x, z, sigmas_f, sigmas_h):
        """
        Compute cross variance of the state `x` and measurement `z`.
        """

        Pxz = zeros((sigmas_f.shape[1], sigmas_h.shape[1]))
        N = sigmas_f.shape[0]
        for i in range(N):
            dx = self.residual_x(sigmas_f[i], x)
            dz = self.residual_z(sigmas_h[i], z)
            Pxz += self.Wc[i] * outer(dx, dz)
        return Pxz

    def compute_process_sigmas(self, 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.
        """

        if fx is None:
            fx = self.fx

        # calculate sigma points for given mean and covariance
        sigmas = self.points_fn.sigma_points(self.x, self.P)

        for i, s in enumerate(sigmas):
            self.sigmas_f[i] = fx(s, dt, **fx_args)

    def batch_filter(self, 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 : bayesian_filters.common.Saver, optional
            bayesian_filters.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
        --------

        .. code-block:: Python

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

        """
        # pylint: disable=too-many-arguments

        try:
            z = zs[0]
        except TypeError:
            raise TypeError("zs must be list-like")

        if self._dim_z == 1:
            if not (isscalar(z) or (z.ndim == 1 and len(z) == 1)):
                raise TypeError("zs must be a list of scalars or 1D, 1 element arrays")
        else:
            if len(z) != self._dim_z:
                raise TypeError("each element in zs must be a 1D array of length {}".format(self._dim_z))

        z_n = len(zs)

        if Rs is None:
            Rs = [self.R] * z_n

        if dts is None:
            dts = [self._dt] * z_n

        # mean estimates from Kalman Filter
        if self.x.ndim == 1:
            means = zeros((z_n, self._dim_x))
        else:
            means = zeros((z_n, self._dim_x, 1))

        # state covariances from Kalman Filter
        covariances = zeros((z_n, self._dim_x, self._dim_x))

        for i, (z, r, dt) in enumerate(zip(zs, Rs, dts)):
            self.predict(dt=dt, UT=UT)
            self.update(z, r, UT=UT)
            means[i, :] = self.x
            covariances[i, :, :] = self.P

            if saver is not None:
                saver.save()

        return (means, covariances)

    def rts_smoother(self, Xs, Ps, Qs=None, dts=None, UT=None):
        """
        Runs the Rauch-Tung-Striebel 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
        --------

        .. code-block:: Python

            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)
        """
        # pylint: disable=too-many-locals, too-many-arguments

        if len(Xs) != len(Ps):
            raise ValueError("Xs and Ps must have the same length")

        n, dim_x = Xs.shape

        if dts is None:
            dts = [self._dt] * n
        elif isscalar(dts):
            dts = [dts] * n

        if Qs is None:
            Qs = [self.Q] * n

        if UT is None:
            UT = unscented_transform

        # smoother gain
        Ks = zeros((n, dim_x, dim_x))

        num_sigmas = self._num_sigmas

        xs, ps = Xs.copy(), Ps.copy()
        sigmas_f = zeros((num_sigmas, dim_x))

        for k in reversed(range(n - 1)):
            # create sigma points from state estimate, pass through state func
            sigmas = self.points_fn.sigma_points(xs[k], ps[k])
            for i in range(num_sigmas):
                sigmas_f[i] = self.fx(sigmas[i], dts[k])

            xb, Pb = UT(sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x)

            # compute cross variance
            Pxb = 0
            for i in range(num_sigmas):
                y = self.residual_x(sigmas_f[i], xb)
                z = self.residual_x(sigmas[i], Xs[k])
                Pxb += self.Wc[i] * outer(z, y)

            # compute gain
            K = dot(Pxb, self.inv(Pb))

            # update the smoothed estimates
            xs[k] += dot(K, self.residual_x(xs[k + 1], xb))
            ps[k] += dot(K, ps[k + 1] - Pb).dot(K.T)
            Ks[k] = K

        return (xs, ps, Ks)

    @property
    def log_likelihood(self):
        """
        log-likelihood of the last measurement.
        """
        if self._log_likelihood is None:
            self._log_likelihood = logpdf(x=self.y, cov=self.S)
        return self._log_likelihood

    @property
    def likelihood(self):
        """
        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.
        """
        if self._likelihood is None:
            self._likelihood = exp(self.log_likelihood)
            if self._likelihood == 0:
                self._likelihood = sys.float_info.min
        return self._likelihood

    @property
    def mahalanobis(self):
        """ "
        Mahalanobis distance of measurement. E.g. 3 means measurement
        was 3 standard deviations away from the predicted value.

        Returns
        -------
        mahalanobis : float
        """
        if self._mahalanobis is None:
            self._mahalanobis = sqrt(dot(dot(self.y.T, self.SI), self.y).item())
        return self._mahalanobis

    def __repr__(self):
        return "\n".join(
            [
                "UnscentedKalmanFilter object",
                pretty_str("x", self.x),
                pretty_str("P", self.P),
                pretty_str("x_prior", self.x_prior),
                pretty_str("P_prior", self.P_prior),
                pretty_str("Q", self.Q),
                pretty_str("R", self.R),
                pretty_str("S", self.S),
                pretty_str("K", self.K),
                pretty_str("y", self.y),
                pretty_str("log-likelihood", self.log_likelihood),
                pretty_str("likelihood", self.likelihood),
                pretty_str("mahalanobis", self.mahalanobis),
                pretty_str("sigmas_f", self.sigmas_f),
                pretty_str("h", self.sigmas_h),
                pretty_str("Wm", self.Wm),
                pretty_str("Wc", self.Wc),
                pretty_str("residual_x", self.residual_x),
                pretty_str("residual_z", self.residual_z),
                pretty_str("msqrt", self.msqrt),
                pretty_str("hx", self.hx),
                pretty_str("fx", self.fx),
                pretty_str("x_mean", self.x_mean),
                pretty_str("z_mean", self.z_mean),
            ]
        )

likelihood property

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.

log_likelihood property

log-likelihood of the last measurement.

mahalanobis property

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

Returns:

Name	Type	Description
mahalanobis	float

__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, state_add=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.

Source code in bayesian_filters/kalman/UKF.py

def __init__(
    self,
    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,
    state_add=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.

    """

    # pylint: disable=too-many-arguments

    self.x = zeros(dim_x)
    self.P = eye(dim_x)
    self.x_prior = np.copy(self.x)
    self.P_prior = np.copy(self.P)
    self.Q = eye(dim_x)
    self.R = eye(dim_z)
    self._dim_x = dim_x
    self._dim_z = dim_z
    self.points_fn = points
    self._dt = dt
    self._num_sigmas = points.num_sigmas()
    self.hx = hx
    self.fx = fx
    self.x_mean = x_mean_fn
    self.z_mean = z_mean_fn

    # Only computed only if requested via property
    self._log_likelihood = log(sys.float_info.min)
    self._likelihood = sys.float_info.min
    self._mahalanobis = None

    if sqrt_fn is None:
        self.msqrt = cholesky
    else:
        self.msqrt = sqrt_fn

    # weights for the means and covariances.
    self.Wm, self.Wc = points.Wm, points.Wc

    if residual_x is None:
        self.residual_x = np.subtract
    else:
        self.residual_x = residual_x

    if residual_z is None:
        self.residual_z = np.subtract
    else:
        self.residual_z = residual_z

    if state_add is None:
        self.state_add = np.add
    else:
        self.state_add = state_add

    # sigma points transformed through f(x) and h(x)
    # variables for efficiency so we don't recreate every update

    self.sigmas_f = zeros((self._num_sigmas, self._dim_x))
    self.sigmas_h = zeros((self._num_sigmas, self._dim_z))

    self.K = np.zeros((dim_x, dim_z))  # Kalman gain
    self.y = np.zeros((dim_z))  # residual
    self.z = np.array([[None] * dim_z]).T  # measurement
    self.S = np.zeros((dim_z, dim_z))  # system uncertainty
    self.SI = np.zeros((dim_z, dim_z))  # inverse system uncertainty

    self.inv = np.linalg.inv

    # these will always be a copy of x,P after predict() is called
    self.x_prior = self.x.copy()
    self.P_prior = self.P.copy()

    # these will always be a copy of x,P after update() is called
    self.x_post = self.x.copy()
    self.P_post = self.P.copy()

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

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

Parameters:

Name	Type	Description	Default
zs	list - like	list of measurements at each time step self._dt Missing measurements must be represented by 'None'.	required
Rs	(None, array or list - like)	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.	None
dts	(None, scalar or list - like)	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.	None
UT	function(sigmas, Wm, Wc, noise_cov)	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.	None
saver	Saver	bayesian_filters.common.Saver object. If provided, saver.save() will be called after every epoch	None

Returns:

Name	Type	Description
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:

.. code-block:: Python

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

Source code in bayesian_filters/kalman/UKF.py

def batch_filter(self, 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 : bayesian_filters.common.Saver, optional
        bayesian_filters.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
    --------

    .. code-block:: Python

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

    """
    # pylint: disable=too-many-arguments

    try:
        z = zs[0]
    except TypeError:
        raise TypeError("zs must be list-like")

    if self._dim_z == 1:
        if not (isscalar(z) or (z.ndim == 1 and len(z) == 1)):
            raise TypeError("zs must be a list of scalars or 1D, 1 element arrays")
    else:
        if len(z) != self._dim_z:
            raise TypeError("each element in zs must be a 1D array of length {}".format(self._dim_z))

    z_n = len(zs)

    if Rs is None:
        Rs = [self.R] * z_n

    if dts is None:
        dts = [self._dt] * z_n

    # mean estimates from Kalman Filter
    if self.x.ndim == 1:
        means = zeros((z_n, self._dim_x))
    else:
        means = zeros((z_n, self._dim_x, 1))

    # state covariances from Kalman Filter
    covariances = zeros((z_n, self._dim_x, self._dim_x))

    for i, (z, r, dt) in enumerate(zip(zs, Rs, dts)):
        self.predict(dt=dt, UT=UT)
        self.update(z, r, UT=UT)
        means[i, :] = self.x
        covariances[i, :, :] = self.P

        if saver is not None:
            saver.save()

    return (means, covariances)

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.

Source code in bayesian_filters/kalman/UKF.py

def compute_process_sigmas(self, 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.
    """

    if fx is None:
        fx = self.fx

    # calculate sigma points for given mean and covariance
    sigmas = self.points_fn.sigma_points(self.x, self.P)

    for i, s in enumerate(sigmas):
        self.sigmas_f[i] = fx(s, dt, **fx_args)

cross_variance(x, z, sigmas_f, sigmas_h)

Compute cross variance of the state x and measurement z.

Source code in bayesian_filters/kalman/UKF.py

def cross_variance(self, x, z, sigmas_f, sigmas_h):
    """
    Compute cross variance of the state `x` and measurement `z`.
    """

    Pxz = zeros((sigmas_f.shape[1], sigmas_h.shape[1]))
    N = sigmas_f.shape[0]
    for i in range(N):
        dx = self.residual_x(sigmas_f[i], x)
        dz = self.residual_z(sigmas_h[i], z)
        Pxz += self.Wc[i] * outer(dx, dz)
    return Pxz

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:

Name	Type	Description	Default
dt	double	If specified, the time step to be used for this prediction. self._dt is used if this is not provided.	None
fx	callable f(x, dt, **fx_args)	State transition function. If not provided, the default function passed in during construction will be used.	None
UT	function(sigmas, Wm, Wc, noise_cov)	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.	None
**fx_args	keyword arguments	optional keyword arguments to be passed into f(x).	{}

Source code in bayesian_filters/kalman/UKF.py

def predict(self, dt=None, UT=None, fx=None, **fx_args):
    r"""
    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, dt, **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).
    """

    if dt is None:
        dt = self._dt

    if UT is None:
        UT = unscented_transform

    # calculate sigma points for given mean and covariance
    self.compute_process_sigmas(dt, fx, **fx_args)

    # and pass sigmas through the unscented transform to compute prior
    self.x, self.P = UT(self.sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x)

    # update sigma points to reflect the new variance of the points
    self.sigmas_f = self.points_fn.sigma_points(self.x, self.P)

    # save prior
    self.x_prior = np.copy(self.x)
    self.P_prior = np.copy(self.P)

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

Runs the Rauch-Tung-Striebel 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:

Name	Type	Description	Default
Xs	array	array of the means (state variable x) of the output of a Kalman filter.	required
Ps	array	array of the covariances of the output of a kalman filter.	required
Qs		Process noise of the Kalman filter at each time step. Optional, if not provided the filter's self.Q will be used	None
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.	required
UT	function(sigmas, Wm, Wc, noise_cov)	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.	None

Returns:

Name	Type	Description
x	ndarray	smoothed means
P	ndarray	smoothed state covariances
K	ndarray	smoother gain at each step

Examples:

.. code-block:: Python

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)

Source code in bayesian_filters/kalman/UKF.py

def rts_smoother(self, Xs, Ps, Qs=None, dts=None, UT=None):
    """
    Runs the Rauch-Tung-Striebel 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
    --------

    .. code-block:: Python

        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)
    """
    # pylint: disable=too-many-locals, too-many-arguments

    if len(Xs) != len(Ps):
        raise ValueError("Xs and Ps must have the same length")

    n, dim_x = Xs.shape

    if dts is None:
        dts = [self._dt] * n
    elif isscalar(dts):
        dts = [dts] * n

    if Qs is None:
        Qs = [self.Q] * n

    if UT is None:
        UT = unscented_transform

    # smoother gain
    Ks = zeros((n, dim_x, dim_x))

    num_sigmas = self._num_sigmas

    xs, ps = Xs.copy(), Ps.copy()
    sigmas_f = zeros((num_sigmas, dim_x))

    for k in reversed(range(n - 1)):
        # create sigma points from state estimate, pass through state func
        sigmas = self.points_fn.sigma_points(xs[k], ps[k])
        for i in range(num_sigmas):
            sigmas_f[i] = self.fx(sigmas[i], dts[k])

        xb, Pb = UT(sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x)

        # compute cross variance
        Pxb = 0
        for i in range(num_sigmas):
            y = self.residual_x(sigmas_f[i], xb)
            z = self.residual_x(sigmas[i], Xs[k])
            Pxb += self.Wc[i] * outer(z, y)

        # compute gain
        K = dot(Pxb, self.inv(Pb))

        # update the smoothed estimates
        xs[k] += dot(K, self.residual_x(xs[k + 1], xb))
        ps[k] += dot(K, ps[k + 1] - Pb).dot(K.T)
        Ks[k] = K

    return (xs, ps, Ks)

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:

Name	Type	Description	Default
z	numpy.array of shape (dim_z)	measurement vector	required
R	array((dim_z, dim_z))	Measurement noise. If provided, overrides self.R for this function call.	None
UT	function(sigmas, Wm, Wc, noise_cov)	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.	None
hx	callable h(x, **hx_args)	Measurement function. If not provided, the default function passed in during construction will be used.	None
**hx_args	keyword argument	arguments to be passed into h(x) after x -> h(x, **hx_args)	{}

Source code in bayesian_filters/kalman/UKF.py

def update(self, 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 : callable h(x, **hx_args), optional
        Measurement function. If not provided, the default
        function passed in during construction will be used.

    **hx_args : keyword argument
        arguments to be passed into h(x) after x -> h(x, **hx_args)
    """

    if z is None:
        self.z = np.array([[None] * self._dim_z]).T
        self.x_post = self.x.copy()
        self.P_post = self.P.copy()
        return

    if hx is None:
        hx = self.hx

    if UT is None:
        UT = unscented_transform

    if R is None:
        R = self.R
    elif isscalar(R):
        R = eye(self._dim_z) * R

    # pass prior sigmas through h(x) to get measurement sigmas
    # the shape of sigmas_h will vary if the shape of z varies, so
    # recreate each time
    sigmas_h = []
    for s in self.sigmas_f:
        sigmas_h.append(hx(s, **hx_args))

    self.sigmas_h = np.atleast_2d(sigmas_h)

    # mean and covariance of prediction passed through unscented transform
    zp, self.S = UT(self.sigmas_h, self.Wm, self.Wc, R, self.z_mean, self.residual_z)
    self.SI = self.inv(self.S)

    # compute cross variance of the state and the measurements
    Pxz = self.cross_variance(self.x, zp, self.sigmas_f, self.sigmas_h)

    self.K = dot(Pxz, self.SI)  # Kalman gain
    self.y = self.residual_z(z, zp)  # residual

    # update Gaussian state estimate (x, P)
    self.x = self.state_add(self.x, dot(self.K, self.y))
    self.P = self.P - dot(self.K, dot(self.S, self.K.T))

    # save measurement and posterior state
    self.z = deepcopy(z)
    self.x_post = self.x.copy()
    self.P_post = self.P.copy()

    # set to None to force recompute
    self._log_likelihood = None
    self._likelihood = None
    self._mahalanobis = None

Merwe Scaled Sigma Points

MerweScaledSigmaPoints

Bases: object

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:

Name	Type	Description	Default
n	int	Dimensionality of the state. 2n+1 weights will be generated.	required
alpha	float	Determins the spread of the sigma points around the mean. Usually a small positive value (1e-3) according to [3].	required
beta	float	Incorporates prior knowledge of the distribution of the mean. For Gaussian x beta=2 is optimal, according to [3].	required
sqrt_method	function(ndarray)	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.	scipy.linalg.cholesky
subtract	callable(x, y)	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.	None

Attributes:

Name	Type	Description
Wm	array	weight for each sigma point for the mean
Wc	array	weight for each sigma point for the covariance

Examples:

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

References

.. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation)

Source code in bayesian_filters/kalman/sigma_points.py

class MerweScaledSigmaPoints(object):
    """
    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].


    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

    Examples
    --------

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


    References
    ----------

    .. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
           Inference in Dynamic State-Space Models" (Doctoral dissertation)

    """

    def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None):
        # pylint: disable=too-many-arguments

        self.n = n
        self.alpha = alpha
        self.beta = beta
        self.kappa = kappa
        if sqrt_method is None:
            self.sqrt = cholesky
        else:
            self.sqrt = sqrt_method

        if subtract is None:
            self.subtract = np.subtract
        else:
            self.subtract = subtract

        self._compute_weights()

    def num_sigmas(self):
        """Number of sigma points for each variable in the state x"""
        return 2 * self.n + 1

    def sigma_points(self, 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 (2n+1, n)
            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}
        """

        if self.n != np.size(x):
            raise ValueError("expected size(x) {}, but size is {}".format(self.n, np.size(x)))

        n = self.n

        if np.isscalar(x):
            x = np.asarray([x])

        if np.isscalar(P):
            P = np.eye(n) * P
        else:
            P = np.atleast_2d(P)

        lambda_ = self.alpha**2 * (n + self.kappa) - n
        U = self.sqrt((lambda_ + n) * P)

        sigmas = np.zeros((2 * n + 1, n))
        sigmas[0] = x
        for k in range(n):
            # pylint: disable=bad-whitespace
            sigmas[k + 1] = self.subtract(x, -U[k])
            sigmas[n + k + 1] = self.subtract(x, U[k])

        return sigmas

    def _compute_weights(self):
        """Computes the weights for the scaled unscented Kalman filter."""

        n = self.n
        lambda_ = self.alpha**2 * (n + self.kappa) - n

        c = 0.5 / (n + lambda_)
        self.Wc = np.full(2 * n + 1, c)
        self.Wm = np.full(2 * n + 1, c)
        self.Wc[0] = lambda_ / (n + lambda_) + (1 - self.alpha**2 + self.beta)
        self.Wm[0] = lambda_ / (n + lambda_)

    def __repr__(self):
        return "\n".join(
            [
                "MerweScaledSigmaPoints object",
                pretty_str("n", self.n),
                pretty_str("alpha", self.alpha),
                pretty_str("beta", self.beta),
                pretty_str("kappa", self.kappa),
                pretty_str("Wm", self.Wm),
                pretty_str("Wc", self.Wc),
                pretty_str("subtract", self.subtract),
                pretty_str("sqrt", self.sqrt),
            ]
        )

num_sigmas()

Number of sigma points for each variable in the state x

Source code in bayesian_filters/kalman/sigma_points.py

def num_sigmas(self):
    """Number of sigma points for each variable in the state x"""
    return 2 * self.n + 1

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:

Name	Type	Description	Default
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])	required
P	scalar, or np.array	Covariance of the filter. If scalar, is treated as eye(n)*P.	required

Returns:

Name	Type	Description
sigmas	np.array, of size (2n+1, n)	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}

Source code in bayesian_filters/kalman/sigma_points.py

def sigma_points(self, 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 (2n+1, n)
        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}
    """

    if self.n != np.size(x):
        raise ValueError("expected size(x) {}, but size is {}".format(self.n, np.size(x)))

    n = self.n

    if np.isscalar(x):
        x = np.asarray([x])

    if np.isscalar(P):
        P = np.eye(n) * P
    else:
        P = np.atleast_2d(P)

    lambda_ = self.alpha**2 * (n + self.kappa) - n
    U = self.sqrt((lambda_ + n) * P)

    sigmas = np.zeros((2 * n + 1, n))
    sigmas[0] = x
    for k in range(n):
        # pylint: disable=bad-whitespace
        sigmas[k + 1] = self.subtract(x, -U[k])
        sigmas[n + k + 1] = self.subtract(x, U[k])

    return sigmas

Julier Sigma Points

JulierSigmaPoints

Bases: object

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:

Name	Type	Description	Default
n	int	Dimensionality of the state. 2n+1 weights will be generated.	required
sqrt_method	function(ndarray)	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.	scipy.linalg.cholesky
subtract	callable(x, y)	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	None

Attributes:

Name	Type	Description
Wm	array	weight for each sigma point for the mean
Wc	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)

Source code in bayesian_filters/kalman/sigma_points.py

class JulierSigmaPoints(object):
    """
    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.


    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)
    """

    def __init__(self, n, kappa=0.0, sqrt_method=None, subtract=None):
        self.n = n
        self.kappa = kappa
        if sqrt_method is None:
            self.sqrt = cholesky
        else:
            self.sqrt = sqrt_method

        if subtract is None:
            self.subtract = np.subtract
        else:
            self.subtract = subtract

        self._compute_weights()

    def num_sigmas(self):
        """Number of sigma points for each variable in the state x"""
        return 2 * self.n + 1

    def sigma_points(self, x, P):
        r""" 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 (2n+1, n)
            2D array of sigma points :math:`\chi`. Each column contains all of
            the sigmas for one dimension in the problem space. They
            are ordered as:

            .. math::
                :nowrap:

                \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}

        """

        if self.n != np.size(x):
            raise ValueError("expected size(x) {}, but size is {}".format(self.n, np.size(x)))

        n = self.n

        if np.isscalar(x):
            x = np.asarray([x])

        n = np.size(x)  # dimension of problem

        if np.isscalar(P):
            P = np.eye(n) * P
        else:
            P = np.atleast_2d(P)

        sigmas = np.zeros((2 * n + 1, n))

        # implements U'*U = (n+kappa)*P. Returns lower triangular matrix.
        # Take transpose so we can access with U[i]
        U = self.sqrt((n + self.kappa) * P)

        sigmas[0] = x
        for k in range(n):
            # pylint: disable=bad-whitespace
            sigmas[k + 1] = self.subtract(x, -U[k])
            sigmas[n + k + 1] = self.subtract(x, U[k])
        return sigmas

    def _compute_weights(self):
        """Computes the weights for the unscented Kalman filter. In this
        formulation the weights for the mean and covariance are the same.
        """

        n = self.n
        k = self.kappa

        self.Wm = np.full(2 * n + 1, 0.5 / (n + k))
        self.Wm[0] = k / (n + k)
        self.Wc = self.Wm

    def __repr__(self):
        return "\n".join(
            [
                "JulierSigmaPoints object",
                pretty_str("n", self.n),
                pretty_str("kappa", self.kappa),
                pretty_str("Wm", self.Wm),
                pretty_str("Wc", self.Wc),
                pretty_str("subtract", self.subtract),
                pretty_str("sqrt", self.sqrt),
            ]
        )

num_sigmas()

Number of sigma points for each variable in the state x

Source code in bayesian_filters/kalman/sigma_points.py

def num_sigmas(self):
    """Number of sigma points for each variable in the state x"""
    return 2 * self.n + 1

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:

Name	Type	Description	Default
x	array-like object of the means of length n	Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2])	required
P	scalar, or np.array	Covariance of the filter. If scalar, is treated as eye(n)*P.	required
kappa	float	Scaling factor.	required

Returns:

Name	Type	Description
sigmas	np.array, of size (2n+1, n)	2D array of sigma points :math:\chi. Each column contains all of the sigmas for one dimension in the problem space. They are ordered as: .. math:: :nowrap: \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}

Source code in bayesian_filters/kalman/sigma_points.py

def sigma_points(self, x, P):
    r""" 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 (2n+1, n)
        2D array of sigma points :math:`\chi`. Each column contains all of
        the sigmas for one dimension in the problem space. They
        are ordered as:

        .. math::
            :nowrap:

            \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}

    """

    if self.n != np.size(x):
        raise ValueError("expected size(x) {}, but size is {}".format(self.n, np.size(x)))

    n = self.n

    if np.isscalar(x):
        x = np.asarray([x])

    n = np.size(x)  # dimension of problem

    if np.isscalar(P):
        P = np.eye(n) * P
    else:
        P = np.atleast_2d(P)

    sigmas = np.zeros((2 * n + 1, n))

    # implements U'*U = (n+kappa)*P. Returns lower triangular matrix.
    # Take transpose so we can access with U[i]
    U = self.sqrt((n + self.kappa) * P)

    sigmas[0] = x
    for k in range(n):
        # pylint: disable=bad-whitespace
        sigmas[k + 1] = self.subtract(x, -U[k])
        sigmas[n + k + 1] = self.subtract(x, U[k])
    return sigmas

Simplex Sigma Points

SimplexSigmaPoints

Bases: object

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

Parameters:

Name	Type	Description	Default
n	int	Dimensionality of the state. n+1 weights will be generated.	required
sqrt_method	function(ndarray)	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.	scipy.linalg.cholesky
subtract	callable(x, y)	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.	None

Attributes:

Name	Type	Description
Wm	array	weight for each sigma point for the mean
Wc	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

Source code in bayesian_filters/kalman/sigma_points.py

class SimplexSigmaPoints(object):
    """
    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
    """

    def __init__(self, n, alpha=1, sqrt_method=None, subtract=None):
        self.n = n
        self.alpha = alpha
        if sqrt_method is None:
            self.sqrt = cholesky
        else:
            self.sqrt = sqrt_method

        if subtract is None:
            self.subtract = np.subtract
        else:
            self.subtract = subtract

        self._compute_weights()

    def num_sigmas(self):
        """Number of sigma points for each variable in the state x"""
        return self.n + 1

    def sigma_points(self, 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+1, n)
            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}
        """

        if self.n != np.size(x):
            raise ValueError("expected size(x) {}, but size is {}".format(self.n, np.size(x)))

        n = self.n

        if np.isscalar(x):
            x = np.asarray([x])
        x = x.reshape(-1, 1)

        if np.isscalar(P):
            P = np.eye(n) * P
        else:
            P = np.atleast_2d(P)

        U = self.sqrt(P)

        lambda_ = n / (n + 1)
        Istar = np.array([[-1 / np.sqrt(2 * lambda_), 1 / np.sqrt(2 * lambda_)]])

        for d in range(2, n + 1):
            row = np.ones((1, Istar.shape[1] + 1)) * 1.0 / np.sqrt(lambda_ * d * (d + 1))  # pylint: disable=unsubscriptable-object
            row[0, -1] = -d / np.sqrt(lambda_ * d * (d + 1))
            Istar = np.r_[np.c_[Istar, np.zeros((Istar.shape[0]))], row]  # pylint: disable=unsubscriptable-object

        I = np.sqrt(n) * Istar
        scaled_unitary = (U.T).dot(I)

        sigmas = self.subtract(x, -scaled_unitary)
        return sigmas.T

    def _compute_weights(self):
        """Computes the weights for the scaled unscented Kalman filter."""

        n = self.n
        c = 1.0 / (n + 1)
        self.Wm = np.full(n + 1, c)
        self.Wc = self.Wm

    def __repr__(self):
        return "\n".join(
            [
                "SimplexSigmaPoints object",
                pretty_str("n", self.n),
                pretty_str("alpha", self.alpha),
                pretty_str("Wm", self.Wm),
                pretty_str("Wc", self.Wc),
                pretty_str("subtract", self.subtract),
                pretty_str("sqrt", self.sqrt),
            ]
        )

num_sigmas()

Number of sigma points for each variable in the state x

Source code in bayesian_filters/kalman/sigma_points.py

def num_sigmas(self):
    """Number of sigma points for each variable in the state x"""
    return self.n + 1

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:

Name	Type	Description	Default
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])	required
P	scalar, or np.array	Covariance of the filter. If scalar, is treated as eye(n)*P.	required

Returns:

Name	Type	Description
sigmas	np.array, of size (n+1, n)	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}

Source code in bayesian_filters/kalman/sigma_points.py

def sigma_points(self, 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+1, n)
        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}
    """

    if self.n != np.size(x):
        raise ValueError("expected size(x) {}, but size is {}".format(self.n, np.size(x)))

    n = self.n

    if np.isscalar(x):
        x = np.asarray([x])
    x = x.reshape(-1, 1)

    if np.isscalar(P):
        P = np.eye(n) * P
    else:
        P = np.atleast_2d(P)

    U = self.sqrt(P)

    lambda_ = n / (n + 1)
    Istar = np.array([[-1 / np.sqrt(2 * lambda_), 1 / np.sqrt(2 * lambda_)]])

    for d in range(2, n + 1):
        row = np.ones((1, Istar.shape[1] + 1)) * 1.0 / np.sqrt(lambda_ * d * (d + 1))  # pylint: disable=unsubscriptable-object
        row[0, -1] = -d / np.sqrt(lambda_ * d * (d + 1))
        Istar = np.r_[np.c_[Istar, np.zeros((Istar.shape[0]))], row]  # pylint: disable=unsubscriptable-object

    I = np.sqrt(n) * Istar
    scaled_unitary = (U.T).dot(I)

    sigmas = self.subtract(x, -scaled_unitary)
    return sigmas.T