�
�g�Uc @` s� d Z d d l m Z m Z m Z m Z d d l Z d d l m Z m
Z
m Z d d l m Z m
Z
m Z d d l m Z m Z m Z d e f d � � YZ d S( 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 ( t absolute_importt divisiont print_functiont unicode_literalsN( t choleskyt qrt pinv( t dott zerost eye( t settert
setter_scalart dot3t SquareRootKalmanFilterc B` sp e Z d d � Z d d � Z d d � Z d � Z d � Z e d � � Z e d � � Z
e j d � � Z e d � � Z e d
� � Z
e j d � � Z e d � � Z e d
� � Z e j d � � Z e d � � Z e j d � � Z e d � � Z e j d � � Z e d � � Z e j d � � Z e d � � Z e j d � � Z e d � � Z e d � � Z RS( i c C` s | d k s t � | d k s$ t � | d k s6 t � | | _ | | _ | | _ t | d f � | _ t | � | _ t | � | _ t | � | _ t | � | _
d | _ d | _ d | _
t | � | _ t | d f � | _ t j | � | _ t j | | | | f � | _ d S( 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.
i i N( t AssertionErrort dim_xt dim_zt dim_uR t _xR t _Pt _P1_2t _Qt _Q1_2t _Bt _Ft _Ht _R1_2t _yt npt _It _M( t selfR R R ( ( s ./filterpy/kalman/square_root.pyt __init__ s$ F c C` s� | d k r d S| d k r( | j } n% t j | � rM t | j � | } n | j } | j } | j | d | � d | � f <t | j | j
� j | | d � d | � f <| j
j | | d � | d � f <t | � \ } } | d | � | d � f j | _ | d | � d | � f j } | t | j | j
� | _ | j
t | j t | � | j � 7_
| | d � | d � f j | _
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 ( t NoneR R t isscalarR R R t TR R R R t _KR R R R ( R t zt R2R t Mt _t St N( ( s ./filterpy/kalman/square_root.pyt update� s" ."" 'c C` s� t | j | j � t | j | � | _ t t j t | j | j � | j g � j
� \ } } | d | j � d | j � f 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( R R t xR R R R t hstackR R R# R ( R t uR# t P2( ( s ./filterpy/kalman/square_root.pyt predict� s (6c C` s | t | j | j � S( um returns the residual for the given measurement (z). Does not alter
the state of the filter.
( R R R ( R R% ( ( s ./filterpy/kalman/square_root.pyt residual_of� s c C` s 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
( R R ( R R, ( ( s ./filterpy/kalman/square_root.pyt measurement_of_state� s
c C` s t | j j | j � S( u Process uncertainty( R R R# ( R ( ( s ./filterpy/kalman/square_root.pyt Q� s c C` s | j S( u Sqrt Process uncertainty( R ( R ( ( s ./filterpy/kalman/square_root.pyt Q1_2� s c C` s1 t | | j � | _ t | j d t �| _ d S( Nt lower( R R R R t TrueR ( R t value( ( s ./filterpy/kalman/square_root.pyR3 � s c C` s t | j j | j � S( u covariance matrix( R R R# ( R ( ( s ./filterpy/kalman/square_root.pyt P� s c C` s | j S( u sqrt of covariance matrix( R ( R ( ( s ./filterpy/kalman/square_root.pyt P1_2� s c C` s1 t | | j � | _ t | j d t �| _ d S( NR5 ( R R R R R6 R ( R R7 ( ( s ./filterpy/kalman/square_root.pyR8 � s c C` s t | j j | j � S( u measurement uncertainty( R R R# ( R ( ( s ./filterpy/kalman/square_root.pyt R� s c C` s | j S( u sqrt of measurement uncertainty( R ( R ( ( s ./filterpy/kalman/square_root.pyt R1_2 s c C` s1 t | | j � | _ t | j d t �| _ d S( NR5 ( R R t _RR R6 R ( R R7 ( ( s ./filterpy/kalman/square_root.pyR:
s c C` s | j S( u Measurement function( R ( R ( ( s ./filterpy/kalman/square_root.pyt H s c C` s t | | j | j � | _ d S( N( R
R R R ( R R7 ( ( s ./filterpy/kalman/square_root.pyR= s c C` s | j S( u state transition matrix( R ( R ( ( s ./filterpy/kalman/square_root.pyt F s c C` s t | | j | j � | _ d S( N( R
R R ( R R7 ( ( s ./filterpy/kalman/square_root.pyR> s c C` s | j S( u control transition matrix( R ( R ( ( s ./filterpy/kalman/square_root.pyt B$ s c C` s t | | j | j � | _ d S( u control transition matrixN( R
R R R ( R R7 ( ( s ./filterpy/kalman/square_root.pyR? * s c C` s | j S( u filter state vector.( R ( R ( ( s ./filterpy/kalman/square_root.pyR, 0 s c C` s t | | j d � | _ d S( Ni ( R
R R ( R R7 ( ( s ./filterpy/kalman/square_root.pyR, 6 s c C` s | j S( u
Kalman gain ( R$ ( R ( ( s ./filterpy/kalman/square_root.pyt K: s c C` s | j S( u# measurement residual (innovation) ( R ( R ( ( s ./filterpy/kalman/square_root.pyt y? s N( t __name__t
__module__R R! R+ R0 R1 R2 t propertyR3 R4 R
R8 R9 R: R; R= R> R? R, R@ RA ( ( ( s ./filterpy/kalman/square_root.pyR
s0 c / ( t __doc__t
__future__R R R R t numpyR t scipy.linalgR R R R R R t filterpy.commonR
R R t objectR
( ( ( s ./filterpy/kalman/square_root.pyt <module> s " 