fading_memory.pyc

�
�g�Uc@`s�dZd
dl
mZmZmZmZ
d
dlZd
dlj	Z	d
dlm
Z
mZmZm
Z

d
dlmZmZmZ
def
d��YZdS(u4
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(tabsolute_importtdivisiontprint_functiontunicode_literalsN(tdottzerosteyetasarray(tsettert
setter_scalartdot3tFadingKalmanFiltercB`s�
eZ
dd
�
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

|
d
kst�
|dks$t�
|dks6t�
|dksHt�
|
d|_
||_||_||_t|d
f�
|_t|�
|_t|�
|_	d|_
d|_d|_t|�
|_
d|_t|d
f�
|_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
        i
iiN(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%
|
d
krd
S|d
kr(|j
}n%tj|�
rMt|j�
|}n|j}|j}|j}|
t	||�|_
t|||j�|}t||jt
j|�
�}|t	||j
�|_|jt	||�}t|||j�t|||j�|_||_||_d
S(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
t
TtlinalgtinvRRR(	Rt
zt
Rt
Ht
Pt
xt
St
KtI_KH((s"./filterpy/kalman/fading_memory.pytupdatess"



	
	
	
+	
cC`s[t|j
|j�t|j|
�|_|jt|j
|j|j
j�|j|_d
S(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(Rt
u((s"./filterpy/kalman/fading_memory.pytpredict�s(cC`s%tj
|
d
�}|dkr.dg
|}nt||jdf�
}t||jdf�
}t||j|jf�
}t||j|jf�
}|rY
xx
tt|
|��
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<qo
W||||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`.
        ii
N(RtsizeR RRt	enumeratetzipR-RRR/(RtzstRstupdate_firstt
ntmeanstmeans_ptcovariancest
covariances_pt
iR%t
r((s"./filterpy/kalman/fading_memory.pytbatch_filter�s.'




(


"


)(



"

&cC`s[t|j
|j�t|j|
�}|jt|j
|j|j
j�|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"t
Q(RR.R)R(((s"./filterpy/kalman/fading_memory.pytget_prediction

s%
,
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_of
scC`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_state
s
c


C`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>2
sc


C`s|jS(
u covariance matrix(
R(
R((s"./filterpy/kalman/fading_memory.pyR(6
scC`st|
|j
�|_dS(
N(R	RR(RRB((s"./filterpy/kalman/fading_memory.pyR(<
sc


C`s|jS(
u measurement uncertainty(
R(
R((s"./filterpy/kalman/fading_memory.pyR&A
scC`st|
|j
�|_dS(
N(R	RR(RRB((s"./filterpy/kalman/fading_memory.pyR&G
sc


C`s|jS(
u Measurement function(
R(
R((s"./filterpy/kalman/fading_memory.pyR'K
scC`st|
|j
|j�|_dS(
N(RRRR(RRB((s"./filterpy/kalman/fading_memory.pyR'Q
sc


C`s|jS(
u state transition matrix(
R(
R((s"./filterpy/kalman/fading_memory.pyt
FV
scC`st|
|j
|j
�|_dS(
N(RRR(RRB((s"./filterpy/kalman/fading_memory.pyRC\
sc


C`s|jS(
u control transition matrix(
R(
R((s"./filterpy/kalman/fading_memory.pyt
B`
scC`st|
|j
|j�|_d
S(u control transition matrixN(RRRR(RRB((s"./filterpy/kalman/fading_memory.pyRDf
sc


C`s|jS(
u filter state vector.(
R(
R((s"./filterpy/kalman/fading_memory.pyt
Xl
scC`st|
|j
d
�|_dS(Ni
(RRR(RRB((s"./filterpy/kalman/fading_memory.pyREr
sc


C`stst
�
dS(
N(tFalseR(
R((s"./filterpy/kalman/fading_memory.pyR)w
sc
C`stst
�
dS(
N(RFR(RRB((s"./filterpy/kalman/fading_memory.pyR){
sc


C`s|jS(
u
 Kalman gain (
R(
R((s"./filterpy/kalman/fading_memory.pyR+
sc


C`s|jS(
u# measurement residual (innovation) (
R(
R((s"./filterpy/kalman/fading_memory.pyt
y�
sc


C`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__RR
RRtnumpyRtscipy.linalgR#RRRRtfilterpy.commonRR	R
tobjectR(((s"./filterpy/kalman/fading_memory.pyt<module>s"

"