Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Download
97429 views
Tweet
Share
Share
�
�g�Uc@`s�dZddlmZmZmZmZddlZddlj	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(tdottzerosteyetasarray(tsettert
setter_scalartdot3tFadingKalmanFiltercB`s�eZdd�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��Zej
d��Zed��Zed��Zed��ZRS(icC`s
|dkst�|dks$t�|dks6t�|dksHt�|d|_||_||_||_t|df�|_t|�|_t|�|_	d|_
d|_d|_t|�|_
d|_t|df�|_d|_tj|�|_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**

        alpha : float, >= 1
            alpha controls how much you want the filter to forget past
            measurements. alpha==1 yields identical performance to the
            Kalman filter. A typical application might use 1.01

        dim_x : int
            Number of state variables for the Kalman 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.


        **Instance Variables**

        You will have to assign reasonable values to all of these before
        running the filter. All must have dtype of float

        X : ndarray (dim_x, 1), default = [0,0,0...0]
            state of the filter

        P : ndarray (dim_x, dim_x), default identity matrix
            covariance matrix

        Q : ndarray (dim_x, dim_x), default identity matrix
            Process uncertainty matrix

        R : ndarray (dim_z, dim_z), default identity matrix
            measurement uncertainty

        H : ndarray (dim_z, dim_x)
            measurement function

        F : ndarray (dim_x, dim_x)
            state transistion matrix

        B : ndarray (dim_x, dim_u), default 0
            control transition matrix
        iiiN(tAssertionErrortalpha_sqtdim_xtdim_ztdim_uRt_XRt_Pt_Qt_Bt_Ft_Ht_Rt_Kt_yt_Stnpt_I(tselftalphaRRR((s"./filterpy/kalman/fading_memory.pyt__init__s&7
								c	C`s%|dkrdS|dkr(|j}n%tj|�rMt|j�|}n|j}|j}|j}|t	||�|_
t|||j�|}t||jt
j|��}|t	||j
�|_|jt	||�}t|||j�t|||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.
        N(tNoneRRtisscalarRRRRRRRR
tTtlinalgtinvRRR(	RtztRtHtPtxtStKtI_KH((s"./filterpy/kalman/fading_memory.pytupdatess"			+	cC`s[t|j|j�t|j|�|_|jt|j|j|jj�|j|_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.
        N(	RRRRR
R
RR"R(Rtu((s"./filterpy/kalman/fading_memory.pytpredict�s(cC`s%tj|d�}|dkr.dg|}nt||jdf�}t||jdf�}t||j|jf�}t||j|jf�}|rYxxtt||��D]�\}	\}
}|j|
|�|j||	dd�f<|j	||	dd�dd�f<|j
�|j||	dd�f<|j	||	dd�dd�f<q�Wn�x�tt||��D]�\}	\}
}|j
�|j||	dd�f<|j	||	dd�dd�f<|j|
|�|j||	dd�f<|j	||	dd�dd�f<qoW||||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 after the update. 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 after the update.
            In other words `covariance[k,:,:]` is the covariance at step `k`.

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

        covariance_predictions: np.array((n,dim_x,dim_x))
            array of the covariances for each time step after the prediction.
            In other words `covariance[k,:,:]` is the covariance at step `k`.
        iiN(RtsizeR RRt	enumeratetzipR-RRR/(RtzstRstupdate_firsttntmeanstmeans_ptcovariancest
covariances_ptiR%tr((s"./filterpy/kalman/fading_memory.pytbatch_filter�s.'("
)(
"&cC`s[t|j|j�t|j|�}|jt|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.
        (	RRRRR
R
RR"tQ(RR.R)R(((s"./filterpy/kalman/fading_memory.pytget_predictions%,cC`s|t|j|j�S(um returns the residual for the given measurement (z). Does not alter
        the state of the filter.
        (RRR(RR%((s"./filterpy/kalman/fading_memory.pytresidual_ofscC`st|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
        (RR(RR)((s"./filterpy/kalman/fading_memory.pytmeasurement_of_states
cC`s|jS(u Process uncertainty(R(R((s"./filterpy/kalman/fading_memory.pyR>,scC`st||j�|_dS(N(R	RR(Rtvalue((s"./filterpy/kalman/fading_memory.pyR>2scC`s|jS(u covariance matrix(R(R((s"./filterpy/kalman/fading_memory.pyR(6scC`st||j�|_dS(N(R	RR(RRB((s"./filterpy/kalman/fading_memory.pyR(<scC`s|jS(u measurement uncertainty(R(R((s"./filterpy/kalman/fading_memory.pyR&AscC`st||j�|_dS(N(R	RR(RRB((s"./filterpy/kalman/fading_memory.pyR&GscC`s|jS(u Measurement function(R(R((s"./filterpy/kalman/fading_memory.pyR'KscC`st||j|j�|_dS(N(RRRR(RRB((s"./filterpy/kalman/fading_memory.pyR'QscC`s|jS(u state transition matrix(R(R((s"./filterpy/kalman/fading_memory.pytFVscC`st||j|j�|_dS(N(RRR(RRB((s"./filterpy/kalman/fading_memory.pyRC\scC`s|jS(u control transition matrix(R(R((s"./filterpy/kalman/fading_memory.pytB`scC`st||j|j�|_dS(u control transition matrixN(RRRR(RRB((s"./filterpy/kalman/fading_memory.pyRDfscC`s|jS(u filter state vector.(R(R((s"./filterpy/kalman/fading_memory.pytXlscC`st||jd�|_dS(Ni(RRR(RRB((s"./filterpy/kalman/fading_memory.pyRErscC`stst�dS(N(tFalseR(R((s"./filterpy/kalman/fading_memory.pyR)wscC`stst�dS(N(RFR(RRB((s"./filterpy/kalman/fading_memory.pyR){scC`s|jS(u
 Kalman gain (R(R((s"./filterpy/kalman/fading_memory.pyR+scC`s|jS(u# measurement residual (innovation) (R(R((s"./filterpy/kalman/fading_memory.pyty�scC`s|jS(u( system uncertainy in measurement space (R(R((s"./filterpy/kalman/fading_memory.pyR*�sN(t__name__t
__module__RR R-R/RFR=R?R@RAtpropertyR>RR(R&R'RCRDRER)R+RGR*(((s"./filterpy/kalman/fading_memory.pyRs4U4I		(t__doc__t
__future__RRRRtnumpyRtscipy.linalgR#RRRRtfilterpy.commonRR	R
tobjectR(((s"./filterpy/kalman/fading_memory.pyt<module>s""