square_root.pyc

�
�g�Uc@`s�dZd
dl
mZmZmZmZ
d
dlZd
dlm	Z	m
Z
mZ
d
dlmZm
Z
mZ
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(tcholeskytqrtpinv(tdottzerosteye(tsettert
setter_scalartdot3tSquareRootKalmanFiltercB`sp
eZ
dd
�
Zdd�
Zdd�
Zd�Zd�Zed��
Z	ed��
Z
e	jd��
Z	ed	��
Zed
��
Z
ejd��
Zed��
Zed
��
Zejd��
Zed��
Zejd��
Zed��
Zejd��
Zed��
Zejd��
Zed��
Zejd��
Zed��
Zed��
ZRS(icC`s
|
d
kst�
|d
ks$t�
|d
ks6t�
|
|_
||_||_t|
df�
|_t|
�
|_t|
�
|_t|
�
|_	t|
�
|_
d
|_d
|_d
|_
t|�
|_t|df�
|_tj|
�
|_tj||
||
f�
|_dS(u�	 Create a Kalman filter which uses a square root implementation.
        This uses the square root of the state covariance matrix, which doubles
        the numerical precision of the filter, Therebuy reducing the effect
        of round off errors.

        It is likely that you do not need to use this algorithm; we understand
        divergence issues very well now. However, if you expect the covariance
        matrix P to vary by 20 or more orders of magnitude then perhaps this
        will be useful to you, as the square root will vary by 10 orders
        of magnitude. From my point of view this is merely a 'reference'
        algorithm; I have not used this code in real world software. Brown[1]
        has a useful discussion of when you might need to use the square
        root form of this algorithm.

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


        **References**

        [1] Robert Grover Brown. Introduction to Random Signals and Applied
            Kalman Filtering. Wiley and sons, 2012.
        ii
N(tAssertionErrortdim_xtdim_ztdim_uRt_xR	t_Pt_P1_2t_Qt_Q1_2t_Bt_Ft_Ht_R1_2t_ytnpt_It_M(tselfRRR((s ./filterpy/kalman/square_root.pyt__init__ s$F

	
	
	




	
	
	
cC`s�
|
d
krd
S|d
kr(|j
}n%tj|�
rMt|j�
|}n|j}|j}|j|d|�d|�f<t|j	|j
�j||d
�d|�f<|j
j||d
�|d
�f<t|�
\}}|d|�|d
�fj|_|d|�d|�fj}|
t|j	|j
�|_|j
t|jt|�
|j�7_
||d
�|d
�fj|_
d
S(u�

        Add a new measurement (z) to the kalman filter. If z is None, nothing
        is changed.

        **Parameters**

        z : np.array
            measurement for this update.

        R2 : np.array, scalar, or None
            Sqrt of meaaurement noize. Optionally provide to override the
            measurement noise for this one call, otherwise  self.R2 will
            be used.
        Ni(tNoneRRtisscalarR	RRt
TRRRRt_KRRRR(Rt
ztR2Rt
Mt
_t
St
N((s ./filterpy/kalman/square_root.pytupdate�s"



	
	
.
"
"
'
cC`s�t|j
|j�t|j|
�|_ttjt|j
|j�|j	g�
j
�
\}}|d
|j�d
|j�fj
|_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(RRt
xRRRRthstackRRR#R(Rt
uR#tP2((s ./filterpy/kalman/square_root.pytpredict�s(6
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/square_root.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/square_root.pytmeasurement_of_state�s
c

C`st|j
j|j
�S(
u Process uncertainty(RRR#(
R((s ./filterpy/kalman/square_root.pyt
Q�sc


C`s|jS(
u Sqrt Process uncertainty(
R(
R((s ./filterpy/kalman/square_root.pytQ1_2�scC`s1t|
|j
�|_t|jd
t�

|_dS(Ntlower(RRRRtTrueR(Rtvalue((s ./filterpy/kalman/square_root.pyR3�s
c

C`st|j
j|j
�S(
u covariance matrix(RRR#(
R((s ./filterpy/kalman/square_root.pyt
P�sc


C`s|jS(
u sqrt of covariance matrix(
R(
R((s ./filterpy/kalman/square_root.pytP1_2�scC`s1t|
|j
�|_t|jd
t�

|_dS(NR5(RRRRR6R(RR7((s ./filterpy/kalman/square_root.pyR8�s
c

C`st|j
j|j
�S(
u measurement uncertainty(RRR#(
R((s ./filterpy/kalman/square_root.pyt
R�sc


C`s|jS(
u  sqrt of measurement uncertainty(
R(
R((s ./filterpy/kalman/square_root.pytR1_2
scC`s1t|
|j
�|_t|jd
t�

|_dS(NR5(RRt_RRR6R(RR7((s ./filterpy/kalman/square_root.pyR:

s
c


C`s|jS(
u Measurement function(
R(
R((s ./filterpy/kalman/square_root.pyt
H
scC`st|
|j
|j�|_dS(
N(R
RRR(RR7((s ./filterpy/kalman/square_root.pyR=
sc


C`s|jS(
u state transition matrix(
R(
R((s ./filterpy/kalman/square_root.pyt
F
scC`st|
|j
|j
�|_dS(
N(R
RR(RR7((s ./filterpy/kalman/square_root.pyR> 
sc


C`s|jS(
u control transition matrix(
R(
R((s ./filterpy/kalman/square_root.pyt
B$
scC`st|
|j
|j�|_d
S(u control transition matrixN(R
RRR(RR7((s ./filterpy/kalman/square_root.pyR?*
sc


C`s|jS(
u filter state vector.(
R(
R((s ./filterpy/kalman/square_root.pyR,0
scC`st|
|j
d
�|_dS(Ni
(R
RR(RR7((s ./filterpy/kalman/square_root.pyR,6
sc


C`s|jS(
u
 Kalman gain (
R$(
R((s ./filterpy/kalman/square_root.pyt
K:
sc


C`s|jS(
u# measurement residual (innovation) (
R(
R((s ./filterpy/kalman/square_root.pyt
y?
sN(t__name__t
__module__R R!R+R0R1R2tpropertyR3R4R
R8R9R:R;R=R>R?R,R@RA(((s ./filterpy/kalman/square_root.pyR
s0c/		(t__doc__t
__future__RR
RRtnumpyRtscipy.linalgRRRRRR	tfilterpy.commonR
RRtobjectR
(((s ./filterpy/kalman/square_root.pyt<module>s"