unscented_transform.pyc

�
�g�Uc@s(dZd
dl
Zdddd�ZdS(s4
Copyright 2015 Roger R Labbe Jr.

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

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

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

This is licensed under an MIT license. See the readme.MD file
for more information.
i����NcCs
|j\}}|d
kr0tj|
|�}n|||
�}|d
kr�||tjd
d
�f}	|	jjtj|�
�
j|	�
}
n]tj||f�
}
xEt|�
D]7}||||�}	|
||tj	|	|	�7}
q�W|d
k	r
|
|7}
n||
fS(s� Computes unscented transform of a set of sigma points and weights.
    returns the mean and covariance in a tuple.

    **Parameters**


    sigamas: ndarray [#sigmas per dimension, dimension]
        2D array of sigma points.

    Wm : ndarray [# sigmas per dimension]
        Weights for the mean. Must sum to 1.


    Wc : ndarray [# sigmas per dimension]
        Weights for the covariance. Must sum to 1.

    noise_cov : ndarray, optional
        noise matrix added to the final computed covariance matrix.

    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

    residual_fn : 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.

        .. 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


    **Returns**

    x : ndarray [dimension]
        Mean of the sigma points after passing through the transform.

    P : ndarray
        covariance of the sigma points after passing throgh the transform.
    N(
tshapetNonetnptdottnewaxist
Ttdiagtzerostrangetouter(tsigmastWmtWct	noise_covtmean_fntresidual_fntkmaxt
nt
xt
yt
Pt
k((s(./filterpy/kalman/unscented_transform.pytunscented_transformsD

'


"

(t__doc__tnumpyRR
R(((s(./filterpy/kalman/unscented_transform.pyt<module>s