Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Download
97429 views
Tweet
Share
Share
�
�g�Uc@`s�dZddlmZmZmZmZddlZddlm	Z	ddlm
Z
mZmZm
Z
ddlmZmZmZdefd��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(tdottzerosteyetasarray(tsettert
setter_scalartdot3tInformationFiltercB`smeZdd�Zdd�Zdd�Zded�Zdd�Zd�Z	d�Z
ed��Zej
d	��Zed
��Zej
d��Zed��Zej
d
��Zed��Zej
d��Zed��Zej
d��Zed��Zej
d��Zed��Zej
d��Zed��Zed��Zed��ZRS(icC`s�|dkst�|dks$t�|dks6t�||_||_||_t|df�|_t|�|_t|�|_d|_	d|_
d|_d|_t|�|_
d|_t|df�|_d|_tj|�|_t|_dS(u! Create a Information filter. You are responsible for setting the
        various state variables to reasonable values; the defaults below will
        not give you a functional filter.

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

            This is used to set the default size of P, Q, and u

        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.

        dim_u : int (optional)
            size of the control input, if it is being used.
            Default value of 0 indicates it is not used.
        iiN(tAssertionErrortdim_xtdim_ztdim_uRt_xRt_P_invt_Qt_Bt_Ft_F_invt_Ht_R_invt_Kt_yt_Stnpt_ItFalset_no_information(tselfRRR((s'./filterpy/kalman/information_filter.pyt__init__s&									cC`sD|dkrdS|dkr(|j}n%tj|�rMt|j�|}n|j}|j}|j}|j	}|j
r�t||�t|||�|_	|t|||�|_n�|t||�|_
|t||�j|�|_tt|j�||�|_|t|j|j
�|_	|t|||�|_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.
        N(tNoneRRtisscalarRRRtTRRRRRRRRR(R tztR_invtHtH_TtP_invtx((s'./filterpy/kalman/information_filter.pytupdateRs$					"c	C`s|t|jj|j|j�}ylt|�}t}|jr�y"tt|j�|j�|_Wntd|j�|_nXt	|_nWnt	}t|_nX|r�t|j
|j�t|j|�|_t||j
�|_n�|jt|j|j�}t|j
j�}t||j�}td|j
�td|�t||j
�}t|t|||��|_dS(u� Predict next position.

        **Parameters**

        u : np.array
            Optional control vector. If non-zero, it is multiplied by B
            to create the control input into the system.
        iuQ=uA=N(RRR$RRtTrueRRRRRR*RRRtprint(	R tutAtAIt
invertabletI_PFtFTItFTIXtAQI((s'./filterpy/kalman/information_filter.pytpredict�s.	"
(
c
C`s|d�tj|d�}|dkr4dg|}nt||jdf�}t||j|jf�}|r�xtt||��D]g\}\}}	|j||	�|j||dd�f<|j	||dd�dd�f<|j
�q�Wn�x~tt||��D]g\}\}}	|j
�|j||	�|j||dd�f<|j	||dd�dd�f<qW||fS(u Batch processes a sequences of measurements.

        **Parameters**

        zs : list-like
            list of measurements at each time step `self.dt` Missing
            measurements must be represented by 'None'.

        Rs : list-like, optional
            optional list of values to use for the measurement error
            covariance; a value of None in any position will cause the filter
            to use `self.R` for that time step.

        update_first : bool, optional,
            controls whether the order of operations is update followed by
            predict, or predict followed by update. Default is predict->update.

        **Returns**

        means: np.array((n,dim_x,1))
            array of the state for each time step. Each entry is an np.array.
            In other words `means[k,:]` is the state at step `k`.

        covariance: np.array((n,dim_x,dim_x))
            array of the covariances for each time step. In other words
            `covariance[k,:,:]` is the covariance at step `k`.
        uthis is not implemented yetiiN(RtsizeR"RRt	enumeratetzipR+Rt_PR6(
R tzstRstupdate_firsttntmeanstcovariancestiR%tr((s'./filterpy/kalman/information_filter.pytbatch_filter�s$("(
&cC`sZd�t|j|j�t|j|�}t|j|j|jj�|j}||fS(u& Predicts the next state of the filter and returns it. Does not
        alter the state of the filter.

        **Parameters**

        u : np.array
            optional control input

        **Returns**

        (x, P)
            State vector and covariance array of the prediction.
        uNot implemented yet(RRRRRR:R$tQ(R R.R*tP((s'./filterpy/kalman/information_filter.pytget_prediction�s%%cC`sd�|t|j|j�S(um returns the residual for the given measurement (z). Does not alter
        the state of the filter.
        uNot implemented yet(RRR(R R%((s'./filterpy/kalman/information_filter.pytresidual_of�scC`sd�t|j|�S(u� Helper function that converts a state into a measurement.

        **Parameters**

        x : np.array
            kalman state vector

        **Returns**

        z : np.array
            measurement corresponding to the given state
        uNot implemented yet(RR(R R*((s'./filterpy/kalman/information_filter.pytmeasurement_of_states
cC`s|jS(u Process uncertainty(R(R ((s'./filterpy/kalman/information_filter.pyRDscC`st||j�|_dS(N(R
RR(R tvalue((s'./filterpy/kalman/information_filter.pyRDscC`s|jS(u inverse covariance matrix(R(R ((s'./filterpy/kalman/information_filter.pyR)scC`st||j�|_dS(N(R
RR(R RI((s'./filterpy/kalman/information_filter.pyR)$scC`s|jS(u measurement uncertainty(R(R ((s'./filterpy/kalman/information_filter.pyR&)scC`st||j�|_dS(N(R
RR(R RI((s'./filterpy/kalman/information_filter.pyR&/scC`s|jS(N(R(R ((s'./filterpy/kalman/information_filter.pyR'3scC`st||j|j�|_dS(N(R	RRR(R RI((s'./filterpy/kalman/information_filter.pyR'8scC`s|jS(N(R(R ((s'./filterpy/kalman/information_filter.pytF=scC`s1t||j|j�|_t|j�|_dS(N(R	RRRR(R RI((s'./filterpy/kalman/information_filter.pyRJBscC`s|jS(u control transition matrix(R(R ((s'./filterpy/kalman/information_filter.pytBGscC`st||j|j�|_dS(u control transition matrixN(R	RRR(R RI((s'./filterpy/kalman/information_filter.pyRKMscC`s|jS(N(R(R ((s'./filterpy/kalman/information_filter.pyR*SscC`st||jd�|_dS(Ni(R	RR(R RI((s'./filterpy/kalman/information_filter.pyR*XscC`s|jS(u
 Kalman gain (R(R ((s'./filterpy/kalman/information_filter.pytK\scC`s|jS(u# measurement residual (innovation) (R(R ((s'./filterpy/kalman/information_filter.pytyascC`s|jS(u( system uncertainy in measurement space (R(R ((s'./filterpy/kalman/information_filter.pytSfsN(t__name__t
__module__R!R"R+R6RRCRFRGRHtpropertyRDR	R)R&R'RJRKR*RLRMRN(((s'./filterpy/kalman/information_filter.pyRs040'=		(t__doc__t
__future__RRRRtnumpyRtscipy.linalgRRRRRtfilterpy.commonR	R
RtobjectR(((s'./filterpy/kalman/information_filter.pyt<module>s""