�
�g�Uc @` s� d Z d d l m Z m Z m Z m Z d d l m Z m Z m Z m
Z
d d l m Z m
Z
d d d � Z d d d � Z d � Z d d d d
� Z 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_literals( t arrayt zerost vstackt eye( t expmt invg �?c C` s� | d k s | d k s t � | d k rm t d | d d | d g d | d | d g g d t �} nb t d | d d | d d | d g d | d | d | g d | d | d g g d t �} | | S( u� Returns the Q matrix for the Discrete Constant White Noise
Model. dim may be either 2 or 3, dt is the time step, and sigma is the
variance in the noise.
Q is computed as the G * G^T * variance, where G is the process noise per
time step. In other words, G = [[.5dt^2][dt]]^T for the constant velocity
model.
**Paramaeters**
dim : int (2 or 3)
dimension for Q, where the final dimension is (dim x dim)
dt : float, default=1.0
time step in whatever units your filter is using for time. i.e. the
amount of time between innovations
var : float, default=1.0
variance in the noise
i i g �?i g �?t dtypei ( t AssertionErrorR t float( t dimt dtt vart Q( ( s# ./filterpy/common/discretization.pyt Q_discrete_white_noise s ''#c C` s� | d k s | d k s t � | d k rc t | d d | d d g | d d | g g � } nv t | d d | d d | d d g | d d | d d | d d g | d d | d d | g g d t �} | | S( u
Returns the Q matrix for the Discretized Continuous White Noise
Model. dim may be either 2 or 3, dt is the time step, and sigma is the
variance in the noise.
**Paramaeters**
dim : int (2 or 3)
dimension for Q, where the final dimension is (dim x dim)
dt : float, default=1.0
time step in whatever units your filter is using for time. i.e. the
amount of time between innovations
spectral_density : float, default=1.0
spectral density for the continuous process
i i i i i i i R
( R R R ( R
R t spectral_densityR ( ( s# ./filterpy/common/discretization.pyt Q_continuous_white_noise< s '$+c C` s | j d } t d | d | f � } | j | � | d | � d | � f <| j | j � j | � | d | � | d | � f <| j j | � | | d | � | d | � f <t | � } | | d | � | d | � f j } | j | d | � | d | � f � } | | f S( u� Discretizes a linear differential equation which includes white noise
according to the method of C. F. van Loan [1]. Given the continuous
model
x' = Fx + Gu
where u is the unity white noise, we compute and return the sigma and Q_k
that discretizes that equation.
**Example**::
Given y'' + y = 2u(t), we create the continuous state model of
x' = | 0 1| * x + |0|*u(t)
|-1 0| |2|
and a time step of 0.1:
>>> F = np.array([[0,1],[-1,0]], dtype=float)
>>> G = np.array([[0.],[2.]])
>>> phi, Q = van_loan_discretization(F, G, 0.1)
>>> phi
array([[ 0.99500417, 0.09983342],
[-0.09983342, 0.99500417]])
>>> Q
array([[ 0.00133067, 0.01993342],
[ 0.01993342, 0.39866933]])
(example taken from Brown[2])
**References**
[1] C. F. van Loan. "Computing Integrals Involving the Matrix Exponential."
IEEE Trans. Automomatic Control, AC-23 (3): 395-404 (June 1978)
[2] Robert Grover Brown. "Introduction to Random Signals and Applied
Kalman Filtering." Forth edition. John Wiley & Sons. p. 126-7. (2012)
i i ( t shapeR t dott TR ( t Ft GR t nt At Bt sigmaR ( ( s# ./filterpy/common/discretization.pyt van_loan_discretizationZ s /
&50 ')i c C` s� | j d } | d k r( t | � } n | d k rI t | | f � } n t | | � } t d | d | f � } | | d | � d | � f <| j | � j | j � | d | � | d | � f <| j | | d | � | d | � f <t t | | f � t | � f � } t | | � j | � } | d | � d d � f } | | d | � d d � f }
| j t |
� � } | | f S( Ni i ( R t NoneR R R R R R R ( R t LR R R R t phit zot CDt Ct Dt q( ( s# ./filterpy/common/discretization.pyt linear_ode_discretation� s
5($ N( t __doc__t
__future__R R R R t numpyR R R R t scipy.linalgR R R R R R R& ( ( ( s# ./filterpy/common/discretization.pyt <module> s ""! C