CoCalc -- sigma_points.pyc

Kalman-and-Bayesian-Filters-in-Python / filterpy / kalman / sigma_points.pyc
⁹⁶¹²⁹ views
�
�g�Uc@sRdZddlZddlmZdefd��YZdefd��YZdS(s4Copyright 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.
i����N(tcholeskytMerweScaledSigmaPointscBs)eZddd�Zd�Zd�ZRS(cCsm||_||_||_||_|dkr<t|_n	||_|dkr`tj|_n	||_dS(s	
        Generates sigma points and weights according to Van der Merwe's
        2004 dissertation [1]. 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.

        **References**

        .. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
               Inference in Dynamic State-Space Models" (Doctoral dissertation)
        N(	tntalphatbetatkappatNoneRtsqrttnptsubtract(tselfRRRRtsqrt_methodR	((s!./filterpy/kalman/sigma_points.pyt__init__s5					cCsD|jtj|�ks<tdj|jtj|����|j}tj|�ritj|g�}ntj|�r�tj|�|}n|jd||j	|}|j
|||�}tjd|d|f�}||d<xUt|�D]G}|j
|||�||d<|j
|||�|||d<q�W|S(s{ 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}
        s expected size {}, but size is {}iii(RRtsizetAssertionErrortformattisscalartasarrayteyeRRRtzerostrangeR	(R
txtPRtlambda_tUtsigmastk((s!./filterpy/kalman/sigma_points.pytsigma_points[s!	
&cCs�|j}|jd||j|}d||}tjd|d|�}tjd|d|�}|||d|jd|j|d<||||d<||fS(s� Computes the weights for the scaled unscented Kalman filter.

        **Returns**

        Wm : ndarray[2n+1]
            weights for mean

        Wc : ndarray[2n+1]
            weights for the covariances
        ig�?ii(RRRRtfullR(R
RRtctWctWm((s!./filterpy/kalman/sigma_points.pytweights�s	(N(t__name__t
__module__RRRR (((s!./filterpy/kalman/sigma_points.pyRsD	2tJulierSigmaPointscBs)eZddd�Zd�Zd�ZRS(cCs[||_||_|dkr*t|_n	||_|dkrNtj|_n	||_dS(s
        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

    **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)
       N(RRRRRRR	(R
RRRR	((s!./filterpy/kalman/sigma_points.pyR�s/			cCs|jtj|�kst�|j}tj|�rKtj|g�}ntj|�}tj|�rtj|�|}ntjd|d|f�}|j||j	|�}||d<xUt
|�D]G}|j|||�||d<|j|||�|||d<q�W|S(s� 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 :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}
        iii(RRR
RRRRRRRRR	(R
RRRRRR((s!./filterpy/kalman/sigma_points.pyR�s&	
&cCsP|j}|j}tjd|dd||�}||||d<||fS(s$ Computes the weights for the unscented Kalman filter. In this
        formulatyion the weights for the mean and covariance are the same.

        **Returns**

        Wm : ndarray[2n+1]
            weights for mean

        Wc : ndarray[2n+1]
            weights for the covariances
        iig�?i(RRRR(R
RRtW((s!./filterpy/kalman/sigma_points.pyR !s
		"N(R!R"RRRR (((s!./filterpy/kalman/sigma_points.pyR#�s<	>(t__doc__tnumpyRtscipy.linalgRtobjectRR#(((s!./filterpy/kalman/sigma_points.pyt<module>s�
Product

Resources

Company
