�
�g�Uc @` s� d Z d d l m Z m Z m Z m Z d d l Z d d l j Z d d l m
Z
m Z m Z d d l
m Z 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 dott zerost eye( t settert setter_1dt
setter_scalart dot3t ExtendedKalmanFilterc B` sI e Z d d � Z d d d d � Z d d d e j d � Z d d � Z d 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 j d � � Z e
d � � Z e
d � � Z e
d � � Z RS( i c C` s� | | _ | | _ t | d f � | _ t | � | _ d | _ d | _ t | � | _ t | � | _ t | d f � | _
t j | � | _ d S( u� Extended 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**
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.
i i N(
t dim_xt dim_zR t _xR t _Pt _Bt _Ft _Rt _Qt _yt npt _I( t selfR R
t dim_u( ( s ./filterpy/kalman/EKF.pyt __init__ s c C` s� t | t � s | f } n t | t � s6 | f } n t j | � rl | j d k rl t j | g t � } n | j } | j } | j } | j
}
| j } | j } | | | � }
t
| | � t
| | � } t | | | j � |
} t |
| |
j � | } t | |
j t j | � � } | t
| | | | | � � | _ | j t
| |
� } t | | | j � t | | | j � | _ d S( u, Performs the predict/update innovation of the extended Kalman
filter.
**Parameters**
z : np.array
measurement for this step.
If `None`, only predict step is perfomed.
HJacobian : function
function which computes the Jacobian of the H matrix (measurement
function). Takes state variable (self.x) as input, along with the
optional arguments in args, and returns H.
Hx : function
function which takes as input the state variable (self.x) along
with the optional arguments in hx_args, and returns the measurement
that would correspond to that state.
args : tuple, optional, default (,)
arguments to be passed into HJacobian after the required state
variable.
hx_args : tuple, optional, default (,)
arguments to be passed into HJacobian after the required state
variable.
u : np.array or scalar
optional control vector input to the filter.
i N( t
isinstancet tupleR t isscalarR
t asarrayt floatR R R R R R R R
t Tt linalgt invR ( R t zt HJacobiant Hxt argst hx_argst ut Ft Bt Pt Qt Rt xt Ht St Kt I_KH( ( s ./filterpy/kalman/EKF.pyt predict_update> s( #c C` sz t | t � s | f } n t | t � s6 | f } n | j } | d k rW | j } n% t j | � r| t | j � | } n t j | � r� | j d k r� t j | g t
� } n | j } | | | � }
t |
| |
j
� | } t | |
j
t j | � � } | | | � }
| | |
� } | t | | � | _ | j t | |
� } t | | | j
� t | | | j
� | _ d S( u� Performs the update innovation of the extended Kalman filter.
**Parameters**
z : np.array
measurement for this step.
If `None`, only predict step is perfomed.
HJacobian : function
function which computes the Jacobian of the H matrix (measurement
function). Takes state variable (self.x) as input, returns H.
Hx : function
function which takes as input the state variable (self.x) along
with the optional arguments in hx_args, and returns the measurement
that would correspond to that state.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
args : tuple, optional, default (,)
arguments to be passed into HJacobian after the required state
variable. for robot localization you might need to pass in
information about the map and time of day, so you might have
`args=(map_data, time)`, where the signature of HCacobian will
be `def HJacobian(x, map, t)`
hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state
variable.
residual : function (z, z2), optional
Optional function that computes the residual (difference) between
the two measurement vectors. If you do not provide this, then the
built in minus operator will be used. You will normally want to use
the built in unless your residual computation is nonlinear (for
example, if they are angles)
i N( R R R t NoneR R R R R
R R R R
R R R! R R ( R R" R# R$ R, R% R& t residualR* R- R. R/ R0 t hxt yR1 ( ( s ./filterpy/kalman/EKF.pyt update~ s( * c C` s, t | j | j � t | j | � | _ d S( u� predicts the next state of X. If you need to
compute the next state yourself, override this function. You would
need to do this, for example, if the usual Taylor expansion to
generate F is not providing accurate results for you. N( R R R R ( R R' ( ( s ./filterpy/kalman/EKF.pyt predict_x� s c C` s9 | j | � t | 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( R8 R
R R R R ( R R' ( ( s ./filterpy/kalman/EKF.pyt predict� s
c C` s | j S( u Process uncertainty( R ( R ( ( s ./filterpy/kalman/EKF.pyR+ � s c C` s t | | j � | _ d S( N( R R R ( R t value( ( s ./filterpy/kalman/EKF.pyR+ � s c C` s | j S( u covariance matrix( R ( R ( ( s ./filterpy/kalman/EKF.pyR* � s c C` s t | | j � | _ d S( N( R R R ( R R: ( ( s ./filterpy/kalman/EKF.pyR* � s c C` s | j S( u measurement uncertainty( R ( R ( ( s ./filterpy/kalman/EKF.pyR, � s c C` s t | | j � | _ d S( N( R R
R ( R R: ( ( s ./filterpy/kalman/EKF.pyR, � s c C` s | j S( N( R ( R ( ( s ./filterpy/kalman/EKF.pyR( � s c C` s t | | j | j � | _ d S( N( R R R ( R R: ( ( s ./filterpy/kalman/EKF.pyR( s c C` s | j S( N( R ( R ( ( s ./filterpy/kalman/EKF.pyR) s c C` s t | | j | j � | _ d S( u control transition matrixN( R R R R ( R R: ( ( s ./filterpy/kalman/EKF.pyR)
s c C` s | j S( N( R ( R ( ( s ./filterpy/kalman/EKF.pyR- s c C` s t | | j � | _ d S( N( R R R ( R R: ( ( s ./filterpy/kalman/EKF.pyR- s c C` s | j S( u
Kalman gain ( t _K( R ( ( s ./filterpy/kalman/EKF.pyR0 s c C` s | j S( u# measurement residual (innovation) ( R ( R ( ( s ./filterpy/kalman/EKF.pyR6 s c C` s | j S( u) system uncertainty in measurement space ( t _S( R ( ( s ./filterpy/kalman/EKF.pyR/ % s ( ( N( ( ( t __name__t
__module__R R2 R3 R t subtractR7 R8 R9 t propertyR+ R R* R, R( R) R- R0 R6 R/ ( ( ( s ./filterpy/kalman/EKF.pyR s* "@ G ( t __doc__t
__future__R R R R t numpyR t scipy.linalgR R R R t filterpy.commonR R R R
t objectR ( ( ( s ./filterpy/kalman/EKF.pyt <module> s " "