# -*- coding: utf-8 -*-
"""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.
"""
#pylint:disable=invalid-name, bad-whitespace
from __future__ import (absolute_import, division, print_function,
unicode_literals)
from numpy import zeros, vstack, eye, array
from numpy.linalg import inv
from scipy.linalg import expm, block_diag
def order_by_derivative(Q, dim, block_size):
"""
Given a matrix Q, ordered assuming state space
[x y z x' y' z' x'' y'' z''...]
return a reordered matrix assuming an ordering of
[ x x' x'' y y' y'' z z' y'']
This works for any covariance matrix or state transition function
Parameters
----------
Q : np.array, square
The matrix to reorder
dim : int >= 1
number of independent state variables. 3 for x, y, z
block_size : int >= 0
Size of derivatives. Second derivative would be a block size of 3
(x, x', x'')
"""
N = dim * block_size
D = zeros((N, N))
Q = array(Q)
for i, x in enumerate(Q.ravel()):
f = eye(block_size) * x
ix, iy = (i // dim) * block_size, (i % dim) * block_size
D[ix:ix+block_size, iy:iy+block_size] = f
return D
[docs]def Q_discrete_white_noise(dim, dt=1., var=1., block_size=1, order_by_dim=True):
"""
Returns the Q matrix for the Discrete Constant White Noise
Model. dim may be either 2, 3, or 4 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.
Parameters
-----------
dim : int (2, 3, or 4)
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
block_size : int >= 1
If your state variable contains more than one dimension, such as
a 3d constant velocity model [x x' y y' z z']^T, then Q must be
a block diagonal matrix.
order_by_dim : bool, default=True
Defines ordering of variables in the state vector. `True` orders
by keeping all derivatives of each dimensions)
[x x' x'' y y' y'']
whereas `False` interleaves the dimensions
[x y z x' y' z' x'' y'' z'']
Examples
--------
>>> # constant velocity model in a 3D world with a 10 Hz update rate
>>> Q_discrete_white_noise(2, dt=0.1, var=1., block_size=3)
array([[0.000025, 0.0005 , 0. , 0. , 0. , 0. ],
[0.0005 , 0.01 , 0. , 0. , 0. , 0. ],
[0. , 0. , 0.000025, 0.0005 , 0. , 0. ],
[0. , 0. , 0.0005 , 0.01 , 0. , 0. ],
[0. , 0. , 0. , 0. , 0.000025, 0.0005 ],
[0. , 0. , 0. , 0. , 0.0005 , 0.01 ]])
References
----------
Bar-Shalom. "Estimation with Applications To Tracking and Navigation".
John Wiley & Sons, 2001. Page 274.
"""
if not (dim == 2 or dim == 3 or dim == 4):
raise ValueError("dim must be between 2 and 4")
if dim == 2:
Q = [[.25*dt**4, .5*dt**3],
[ .5*dt**3, dt**2]]
elif dim == 3:
Q = [[.25*dt**4, .5*dt**3, .5*dt**2],
[ .5*dt**3, dt**2, dt],
[ .5*dt**2, dt, 1]]
else:
Q = [[(dt**6)/36, (dt**5)/12, (dt**4)/6, (dt**3)/6],
[(dt**5)/12, (dt**4)/4, (dt**3)/2, (dt**2)/2],
[(dt**4)/6, (dt**3)/2, dt**2, dt],
[(dt**3)/6, (dt**2)/2 , dt, 1.]]
if order_by_dim:
return block_diag(*[Q]*block_size) * var
return order_by_derivative(array(Q), dim, block_size) * var
[docs]def Q_continuous_white_noise(dim, dt=1., spectral_density=1.,
block_size=1, order_by_dim=True):
"""
Returns the Q matrix for the Discretized Continuous White Noise
Model. dim may be either 2, 3, 4, dt is the time step, and sigma is the
variance in the noise.
Parameters
----------
dim : int (2 or 3 or 4)
dimension for Q, where the final dimension is (dim x dim)
2 is constant velocity, 3 is constant acceleration, 4 is
constant jerk
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
block_size : int >= 1
If your state variable contains more than one dimension, such as
a 3d constant velocity model [x x' y y' z z']^T, then Q must be
a block diagonal matrix.
order_by_dim : bool, default=True
Defines ordering of variables in the state vector. `True` orders
by keeping all derivatives of each dimensions)
[x x' x'' y y' y'']
whereas `False` interleaves the dimensions
[x y z x' y' z' x'' y'' z'']
Examples
--------
>>> # constant velocity model in a 3D world with a 10 Hz update rate
>>> Q_continuous_white_noise(2, dt=0.1, block_size=3)
array([[0.00033333, 0.005 , 0. , 0. , 0. , 0. ],
[0.005 , 0.1 , 0. , 0. , 0. , 0. ],
[0. , 0. , 0.00033333, 0.005 , 0. , 0. ],
[0. , 0. , 0.005 , 0.1 , 0. , 0. ],
[0. , 0. , 0. , 0. , 0.00033333, 0.005 ],
[0. , 0. , 0. , 0. , 0.005 , 0.1 ]])
"""
if not (dim == 2 or dim == 3 or dim == 4):
raise ValueError("dim must be between 2 and 4")
if dim == 2:
Q = [[(dt**3)/3., (dt**2)/2.],
[(dt**2)/2., dt]]
elif dim == 3:
Q = [[(dt**5)/20., (dt**4)/8., (dt**3)/6.],
[ (dt**4)/8., (dt**3)/3., (dt**2)/2.],
[ (dt**3)/6., (dt**2)/2., dt]]
else:
Q = [[(dt**7)/252., (dt**6)/72., (dt**5)/30., (dt**4)/24.],
[(dt**6)/72., (dt**5)/20., (dt**4)/8., (dt**3)/6.],
[(dt**5)/30., (dt**4)/8., (dt**3)/3., (dt**2)/2.],
[(dt**4)/24., (dt**3)/6., (dt**2/2.), dt]]
if order_by_dim:
return block_diag(*[Q]*block_size) * spectral_density
return order_by_derivative(array(Q), dim, block_size) * spectral_density
[docs]def van_loan_discretization(F, G, dt):
"""
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.
Examples
--------
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)
"""
n = F.shape[0]
A = zeros((2*n, 2*n))
# we assume u(t) is unity, and require that G incorporate the scaling term
# for the noise. Hence W = 1, and GWG' reduces to GG"
A[0:n, 0:n] = -F.dot(dt)
A[0:n, n:2*n] = G.dot(G.T).dot(dt)
A[n:2*n, n:2*n] = F.T.dot(dt)
B=expm(A)
sigma = B[n:2*n, n:2*n].T
Q = sigma.dot(B[0:n, n:2*n])
return (sigma, Q)
[docs]def linear_ode_discretation(F, L=None, Q=None, dt=1.):
n = F.shape[0]
if L is None:
L = eye(n)
if Q is None:
Q = zeros((n, n))
A = expm(F * dt)
phi = zeros((2*n, 2*n))
phi[0:n, 0:n] = F
phi[0:n, n:2*n] = L.dot(Q).dot(L.T)
phi[n:2*n, n:2*n] = -F.T
zo = vstack((zeros((n, n)), eye(n)))
CD = expm(phi*dt).dot(zo)
C = CD[0:n, :]
D = CD[n:2*n, :]
q = C.dot(inv(D))
return (A, q)