Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Download
96129 views
Tweet
Share
Share
�
�g�Uc@`s�dZddlmZmZmZmZddlZddlm	Z	ddlm
Z
mZmZm
Z
ddlmZddlmZdefd	��YZdS(
u4Copyright 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(tabsolute_importtdivisiontprint_functiontunicode_literalsN(tinv(tdottzerosteyetouter(tmultivariate_normal(tdot3tEnsembleKalmanFiltercB`s5eZdZd�Zd�Zdd�Zd�ZRS(u� This implements the ensemble Kalman filter (EnKF). The EnKF uses
    an ensemble of hundreds to thousands of state vectors that are randomly
    sampled around the estimate, and adds perturbations at each update and
    predict step. It is useful for extremely large systems such as found
    in hydrophysics. As such, this class is admittedly a toy as it is far
    too slow with large N.

    There are many versions of this sort of this filter. This formulation is
    due to Crassidis and Junkins [1]. It works with both linear and nonlinear
    systems.

    **References**

    - [1] John L Crassidis and John L. Junkins. "Optimal Estimation of
      Dynamic Systems. CRC Press, second edition. 2012. pp, 257-9.
    cC`s�|dkst�t|�|_||_||_||_||_||_t|j�|_	t|j�|_
dg|j|_|j||�dS(u� 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.

        **Parameters**

        x : np.array(dim_z)
            state mean

        P : np.array((dim_x, dim_x))
            covariance of the state

        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.

        dt : float
            time step in seconds

        N : int
            number of sigma points (ensembles). Must be greater than 1.

        hx : function hx(x)
            Measurement function. May be linear or nonlinear - converts state
            x into a measurement. Return must be an np.array of the same
            dimensionality as the measurement vector.

        fx : function fx(x, dt)
            State transition function. May be linear or nonlinear. Projects
            state x into the next time period. Returns the projected state x.

        **Example**

        .. code::

            def hx(x):
               return np.array([x[0]])

            F = np.array([[1., 1.],
                          [0., 1.]])
            def fx(x, dt):
                return np.dot(F, x)

            x = np.array([0., 1.])
            P = np.eye(2) * 100.
            dt = 0.1
            f = EnKF(x=x, P=P, dim_z=1, dt=dt, N=8,
                     hx=hx, fx=fx)

            std_noise = 3.
            f.R *= std_noise**2
            f.Q = Q_discrete_white_noise(2, dt, .01)

            while True:
                z = read_sensor()
                f.predict()
                f.update(np.asarray([z]))

        iN(
tAssertionErrortlentdim_xtdim_ztdttNthxtfxRtQtRtmeant
initialize(tselftxtPRRRRR((s+./filterpy/kalman/ensemble_kalman_filter.pyt__init__.s=					cC`sL|jdkst�td|d|d|j�|_||_||_dS(ua Initializes the filter with the specified mean and
        covariance. Only need to call this if you are using the filter
        to filter more than one set of data; this is called by __init__

        **Parameters**

        x : np.array(dim_z)
            state mean

        P : np.array((dim_x, dim_x))
            covariance of the state
        iRtcovtsizeN(tndimRR	RtsigmasRR(RRR((s+./filterpy/kalman/ensemble_kalman_filter.pyRzs
!	c	C`s|dkrdS|dkr(|j}ntj|�rMt|j�|}n|j}t|�}t||f�}x.t	|�D] }|j
|j|�||<q�Wtj|dd�}d}x+|D]#}	|	|}
|t
|
|
�7}q�W||d|}d}x=t	|�D]/}|t
|j||j|||�7}qW||d}t|t|��}tdg|||�}
x@t	|�D]2}|j|ct|||
|||�7<q�Wtj|jdd�|_|jt|||j�|_dS(uk
        Add a new measurement (z) to the kalman filter. If z is None, nothing
        is changed.

        **Parameters**

        z : np.array
            measurement for this update.

        R : np.array, scalar, or None
            Optionally provide R to override the measurement noise for this
            one call, otherwise  self.R will be used.
        Ntaxisii(tNoneRtnptisscalarRRRR
RtrangeRRRRRRRR	RR
tT(RtzRRRtsigmas_htitz_meantP_zztsigmatstP_xztKte_r((s+./filterpy/kalman/ensemble_kalman_filter.pytupdate�s6	

-0cC`s�|j}x9t|j�D](\}}|j||j�|j|<qWt|j|j|�}|j|7_d}x1|jD]&}||j}|t	||�7}q|W||d|_
dS(u Predict next position. iiN(Rt	enumerateRRRR	RRRRR(RRR(R,teRtsx((s+./filterpy/kalman/ensemble_kalman_filter.pytpredict�s	 
N(t__name__t
__module__t__doc__RRR!R0R4(((s+./filterpy/kalman/ensemble_kalman_filter.pyRs
	L	6(R7t
__future__RRRRtnumpyR"tscipy.linalgRRRRRtnumpy.randomR	tfilterpy.commonR
tobjectR(((s+./filterpy/kalman/ensemble_kalman_filter.pyt<module>s""