�
�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 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 eyet isscalar( t settert setter_1dt
setter_scalart dot3t KalmanFilterc B` sj e Z d Z d d � Z d d d � Z d � Z d d � Z d e d � Z d d � Z
d d � Z d � Z 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( ui Implements a Kalman filter. You are responsible for setting the
various state variables to reasonable values; the defaults will
not give you a functional filter.
You will have to set the following attributes after constructing this
object for the filter to perform properly. Please note that there are
various checks in place to ensure that you have made everything the
'correct' size. However, it is possible to provide incorrectly sized
arrays such that the linear algebra can not perform an operation.
It can also fail silently - you can end up with matrices of a size that
allows the linear algebra to work, but are the wrong shape for the problem
you are trying to solve.
**Attributes**
x : numpy.array(dim_x, 1)
state estimate vector
P : numpy.array(dim_x, dim_x)
covariance estimate matrix
R : numpy.array(dim_z, dim_z)
measurement noise matrix
Q : numpy.array(dim_x, dim_x)
process noise matrix
F : numpy.array()
State Transition matrix
H : numpy.array(dim_x, dim_x)
You may read the following attributes.
**Readable Attributes**
y : numpy.array
Residual of the update step.
K : numpy.array(dim_x, dim_z)
Kalman gain of the update step
S : numpy.array
Systen uncertaintly projected to measurement space
i c C` s� | d k s t � | d k s$ t � | d k s6 t � | | _ | | _ | | _ t | d f � | _ t | � | _ t | � | _ d | _ d | _
d | _ t | � | _ d | _
d | _ t | d f � | _ d | _ t j | � | _ d S( 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**
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.
i i g �?N( t AssertionErrort dim_xt dim_zt dim_uR t _xR t _Pt _Qt _Bt _Ft Ht Rt _alpha_sqt _Kt _yt _St npt _I( t selfR R R ( ( s" ./filterpy/kalman/kalman_filter.pyt __init__N s$ c C` s1 | d k r d S| d k r( | j } n" t | � rJ t | j � | } n | d k rb | j } n | j } | j } | t | | � | _ t
| | | j � | } t
| | j t j
| � � } | t | | j � | _ | j t | | � } 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( t NoneR R R R R R R R R R t Tt linalgt invR R R ( R t zR R t Pt xt St Kt I_KH( ( s" ./filterpy/kalman/kalman_filter.pyt update� s$ + c C` s� | j j | j d f k s? t d j | j d | j j � � � | j j | j | j f k s� t d j | j | j | j j � � � | j j | j | j f k s� t d j | j | j | j j � � � d S( u Performs a series of asserts to check that the size of everything
is what it should be. This can help you debug problems in your design.
This is only a test; you do not need to use it while filtering.
However, to use you will want to perform at least one predict() and
one update() before calling; some bad designs will cause the shapes
of x and P to change in a silent and bad way. For example, if you
pass in a badly dimensioned z into update that can cause x to be
misshapen.i u% Shape of x must be ({},{}), but is {}u% Shape of P must be ({},{}), but is {}N( R t shapeR R
t formatR R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyt test_matrix_dimensions� s ! !c C` s[ t | j | j � t | j | � | _ | 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(
R R R& R R R R R R! R ( R t u( ( s" ./filterpy/kalman/kalman_filter.pyt predict� s (c C` s y | d } Wn t | � s1 t d � � n X| j d k r| t | � s� | j d k rm t | � d k s� t d � � n1 t | � | j k s� t d � � d j | j � t j | d � } | d k r� d g | } n | j j d k rt
| | j f � } t
| | j f � } n0 t
| | j d f � } t
| | j d f � } t
| | j | j f � } t
| | j | j f � } | rExxt t
| | � � D]� \ }
\ } } | j | | � | j | |
d d � f <| j | |
d d � d d � f <| j � | j | |
d d � f <| j | |
d d � d d � f <q�Wn� x� t t
| | � � D]� \ }
\ } } | j � | j | |
d d � f <| j | |
d d � d d � f <| j | | � | j | |
d d � f <| j | |
d d � d d � f <q[W| | | | f S( 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`.
i u zs must be list-likei u4 zs must be a list of scalars or 1D, 1 element arraysu each element in zs must be au 1D array of length {}N( R R
R t ndimt lenR, R t sizeR R& R R t enumeratet zipR* R R R/ ( R t zst Rst update_firstR$ t nt meanst means_pt covariancest
covariances_pt it r( ( s" ./filterpy/kalman/kalman_filter.pyt batch_filter� sF ( 0 ! ("
)(
"&c
C` si t | � t | � k s t � | j } | d } | d } | j } | s] | j g | } n t | | | f � } | j � | j � } }
x� t | d d d � D]� } t | |
| | j � | | } t |
| | j t
j | � � | | <| | c t | | | | d t | | | � � 7<|
| c t | | |
| d | | | j � 7<q� W| |
| f S( u" Runs the Rauch-Tung-Striebal Kalman smoother on a set of
means and covariances computed by a Kalman filter. The usual input
would come from the output of `KalmanFilter.batch_filter()`.
**Parameters**
Xs : numpy.array
array of the means (state variable x) of the output of a Kalman
filter.
Ps : numpy.array
array of the covariances of the output of a kalman filter.
Q : list-like collection of numpy.array, optional
Process noise of the Kalman filter at each time step. Optional,
if not provided the filter's self.Q will be used
**Returns**
'x' : numpy.ndarray
smoothed means
'P' : numpy.ndarray
smoothed state covariances
'K' : numpy.ndarray
smoother gain at each step
**Example**::
zs = [t + random.randn()*4 for t in range (40)]
(mu, cov, _, _) = kalman.batch_filter(zs)
(x, P, K) = rts_smoother(mu, cov, fk.F, fk.Q)
i i i i����(
R1 R
R+ R t QR t copyt rangeR R! R" R# R (
R t Xst Pst QsR+ R8 R t FR( R& R% t kt P_pred( ( s" ./filterpy/kalman/kalman_filter.pyt rts_smoother; s (
!&67c C` s[ t | j | j � t | j | � } | j t | j | j | j j � | j } | | f S( 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.
( R R R R R R R R! R ( R R. R& R% ( ( s" ./filterpy/kalman/kalman_filter.pyt get_prediction{ s %,c 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/kalman_filter.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/kalman_filter.pyt measurement_of_state� s c C` s | j d S( u� Fading memory setting. 1.0 gives the normal Kalman filter, and
values slightly larger than 1.0 (such as 1.02) give a fading
memory effect - previous measurements have less influence on the
filter's estimates. This formulation of the Fading memory filter
(there are many) is due to Dan Simon [1].
** References **
[1] Dan Simon. "Optimal State Estimation." John Wiley & Sons.
p. 208-212. (2006)
g �?( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyt alpha� s c C` s8 t j | � s t � | d k s' t � | d | _ d S( Ni i ( R R R
R ( R t value( ( s" ./filterpy/kalman/kalman_filter.pyRM � s c C` s | j S( u Process uncertainty( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyR@ � s c C` s t | | j � | _ d S( N( R
R R ( R RN ( ( s" ./filterpy/kalman/kalman_filter.pyR@ � s c C` s | j S( u covariance matrix( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyR% � s c C` s t | | j � | _ d S( N( R
R R ( R RN ( ( s" ./filterpy/kalman/kalman_filter.pyR% � s c C` s | j S( u state transition matrix( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyRF � s c C` s t | | j | j � | _ d S( N( R R R ( R RN ( ( s" ./filterpy/kalman/kalman_filter.pyRF � s c C` s | j S( u control transition matrix( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyt B� s c C` s: t j | � r | | _ n t | | j | j � | _ d S( u control transition matrixN( R R R R R R ( R RN ( ( s" ./filterpy/kalman/kalman_filter.pyRO � s c C` s | j S( u filter state vector.( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyR& � s c C` s t | | j � | _ d S( N( R R R ( R RN ( ( s" ./filterpy/kalman/kalman_filter.pyR& � s c C` s | j S( u
Kalman gain ( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyR( � s c C` s | j S( u# measurement residual (innovation) ( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyt y� s c C` s | j S( u) system uncertainty in measurement space ( R ( R ( ( s" ./filterpy/kalman/kalman_filter.pyR' s N( t __name__t
__module__t __doc__R R R* R- R/ t FalseR? RI RJ RK RL t propertyRM R R@ R% RF RO R& R( RP R' ( ( ( s" ./filterpy/kalman/kalman_filter.pyR s2 / 35 \ @ ( RS t
__future__R R R R t numpyR t scipy.linalgR" R R R R t filterpy.commonR R R
R t objectR ( ( ( s" ./filterpy/kalman/kalman_filter.pyt <module> s " ""