UnscentedKalmanFilter¶
Introduction and Overview¶
This implements the unscented Kalman filter.
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/KalmanandBayesianFiltersinPython
This is licensed under an MIT license. See the readme.MD file for more information.

class
filterpy.kalman.
UnscentedKalmanFilter
(dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None)[source]¶ Implements the Scaled Unscented Kalman filter (UKF) as defined by Simon Julier in [1], using the formulation provided by Wan and Merle in [2]. This filter scales the sigma points to avoid strong nonlinearities.
You will have to set the following attributes after constructing this object for the filter to perform properly.
References
[R1] Julier, Simon J. “The scaled unscented transformation,” American Control Converence, 2002, pp 45554559, vol 6.
Online copy: https://www.cs.unc.edu/~welch/kalman/media/pdf/ACC02IEEE1357.PDF
[R2] E. A. Wan and R. Van der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000.
Online Copy: https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf
Attributes
x (numpy.array(dim_x)) 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 You may read the following attributes. 
__init__
(dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None)[source]¶ 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 filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4.
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.
dt : float
Time between steps in seconds.
hx : function(x)
Measurement function. Converts state vector x into a measurement vector of shape (dim_z).
fx : function(x,dt)
function that returns the state x transformed by the state transistion function. dt is the time step in seconds.
points : class
Class which computes the sigma points and weights for a UKF algorithm. You can vary the UKF implementation by changing this class. For example, MerweScaledSigmaPoints implements the alpha, beta, kappa parameterization of Van der Merwe, and JulierSigmaPoints implements Julier’s original kappa parameterization. See either of those for the required signature of this class if you want to implement your own.
sqrt_fn : callable(ndarray), default = scipy.linalg.cholesky
Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe’s dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you.
If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky  for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing.
x_mean_fn : callable (sigma_points, weights), optional
Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed.
def state_mean(sigmas, Wm): x = np.zeros(3) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += sin(s[2])*Wm[i] sum_cos += cos(s[2])*Wm[i] x[2] = atan2(sum_sin, sum_cos) return x
z_mean_fn : callable (sigma_points, weights), optional
Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx().
residual_x : callable (x, y), optional
residual_z : callable (x, y), optional
Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (3591 degreees is 2, not 358). x and y are state vectors, not scalars. One is for the state variable, the other is for the measurement state.
def residual(a, b): y = a[0]  b[0] if y > np.pi: y = 2*np.pi if y < np.pi: y = 2*np.pi return y
References
[R7] S. Julier, J. Uhlmann, and H. DurrantWhyte. “A new method for the nonlinear transformation of means and covariances in filters and estimators,” IEEE Transactions on Automatic Control, 45(3), pp. 477482 (March 2000). [R8] E. A. Wan and R. Van der Merwe, “The Unscented Kalman filter for Nonlinear Estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000.
https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf
[R9] Wan, Merle “The Unscented Kalman Filter,” chapter in Kalman Filtering and Neural Networks, John Wiley & Sons, Inc., 2001. [R10] R. Van der Merwe “SigmaPoint Kalman Filters for Probabilitic Inference in Dynamic StateSpace Models” (Doctoral dissertation)

predict
(dt=None, UT=None, fx_args=())[source]¶ Performs the predict step of the UKF. On return, self.x and self.P contain the predicted state (x) and covariance (P). ‘
Important: this MUST be called before update() is called for the first time.
Parameters: dt : double, optional
If specified, the time step to be used for this prediction. self._dt is used if this is not provided.
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work  you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform.
fx_args : tuple, optional, default (,)
optional arguments to be passed into fx() after the required state variable.

update
(z, R=None, UT=None, hx_args=())[source]¶ Update the UKF with the given measurements. On return, self.x and self.P contain the new mean and covariance of the filter.
Parameters: z : numpy.array of shape (dim_z)
measurement vector
R : numpy.array((dim_z, dim_z)), optional
Measurement noise. If provided, overrides self.R for this function call.
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work  you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform.
hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state variable.

batch_filter
(zs, Rs=None, UT=None)[source]¶ Performs the UKF filter over the list of measurement in zs.
Parameters: zs : listlike
list of measurements at each time step self._dt Missing measurements must be represented by ‘None’.
Rs : listlike, 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.
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work  you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform.
Returns: means: ndarray((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: ndarray((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.

rts_smoother
(Xs, Ps, Qs=None, dt=None)[source]¶ Runs the RauchTungStriebal Kalman smoother on a set of means and covariances computed by the UKF. The usual input would come from the output of 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.
Qs: listlike 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
dt : optional, float or arraylike of float
If provided, specifies the time step of each step of the filter. If float, then the same time step is used for all steps. If an array, then each element k contains the time at step k. Units are seconds.
Returns: x : numpy.ndarray
smoothed means
P : numpy.ndarray
smoothed state covariances
K : numpy.ndarray
smoother gain at each step
Examples
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)


class
filterpy.kalman.
MerweScaledSigmaPoints
(n, alpha, beta, kappa, sqrt_method=None, subtract=None)[source]¶ 
__init__
(n, alpha, beta, kappa, sqrt_method=None, subtract=None)[source]¶ Generates sigma points and weights according to Van der Merwe’s 2004 dissertation[1]. It parametizes the sigma points using alpha, beta, kappa terms, and is the version seen in most publications.
Unless you know better, this should be your default choice.
Parameters: n : int
Dimensionality of the state. 2n+1 weights will be generated.
alpha : float
Determins the spread of the sigma points around the mean. Usually a small positive value (1e3) according to [3].
beta : float
Incorporates prior knowledge of the distribution of the mean. For Gaussian x beta=2 is optimal, according to [3].
kappa : float, default=0.0
Secondary scaling parameter usually set to 0 according to [4], or to 3n according to [5].
sqrt_method : function(ndarray), default=scipy.linalg.cholesky
Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe’s dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you.
If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky  for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing.
subtract : callable (x, y), optional
Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (3591 degreees is 2, not 358). x and y are state vectors, not scalars.
References
[R9] R. Van der Merwe “SigmaPoint Kalman Filters for Probabilitic Inference in Dynamic StateSpace Models” (Doctoral dissertation)

sigma_points
(x, P)[source]¶ Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights.
Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I
Parameters: X An arraylike object of the means of length n
Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2])
P : scalar, or np.array
Covariance of the filter. If scalar, is treated as eye(n)*P.
Returns: sigmas : np.array, of size (n, 2n+1)
Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space.
Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}


class
filterpy.kalman.
JulierSigmaPoints
(n, kappa, sqrt_method=None, subtract=None)[source]¶ 
__init__
(n, kappa, sqrt_method=None, subtract=None)[source]¶ Generates sigma points and weights according to Simon J. Julier and Jeffery K. Uhlmann’s original paper []. It parametizes the sigma points using kappa.
Parameters: n : int
Dimensionality of the state. 2n+1 weights will be generated.
kappa : float, default=0.
Scaling factor that can reduce high order errors. kappa=0 gives the standard unscented filter. According to [Julier], if you set kappa to 3dim_x for a Gaussian x you will minimize the fourth order errors in x and P.
sqrt_method : function(ndarray), default=scipy.linalg.cholesky
Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe’s dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you.
If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky  for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing.
subtract : callable (x, y), optional
Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (3591 degreees is 2, not 358). x and y
References
[R10] Julier, Simon J.; Uhlmann, Jeffrey “A New Extension of the Kalman Filter to Nonlinear Systems”. Proc. SPIE 3068, Signal Processing, Sensor Fusion, and Target Recognition VI, 182 (July 28, 1997)

sigma_points
(x, P)[source]¶ Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. kappa is an arbitrary constant. Returns sigma points.
Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I
Parameters: X : arraylike object of the means of length n
Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2])
P : scalar, or np.array
Covariance of the filter. If scalar, is treated as eye(n)*P.
kappa : float
Scaling factor.
Returns: sigmas : np.array, of size (n, 2n+1)
2D array of sigma points \(\chi\). Each column contains all of the sigmas for one dimension in the problem space. They are ordered as:
\begin{eqnarray} \chi[0] = &x \\ \chi[1..n] = &x + [\sqrt{(n+\kappa)P}]_k \\ \chi[n+1..2n] = &x  [\sqrt{(n+\kappa)P}]_k \end{eqnarray}


class
filterpy.kalman.
SimplexSigmaPoints
(n, alpha=1, sqrt_method=None, subtract=None)[source]¶ 
__init__
(n, alpha=1, sqrt_method=None, subtract=None)[source]¶ Generates sigma points and weights according to the simplex method presented in [1] DOI: 10.1051/cocv/2010006
Parameters: n : int
Dimensionality of the state. n+1 weights will be generated.
sqrt_method : function(ndarray), default=scipy.linalg.cholesky
Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm
If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky  for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing.
subtract : callable (x, y), optional
Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (3591 degreees is 2, not 358). x and y are state vectors, not scalars.
References
[R11] Phillippe Moireau and Dominique Chapelle “ReducedOrder Unscented Kalman Filtering with Application to Parameter Identification in LargeDimensional Systems”

sigma_points
(x, P)[source]¶ Computes the implex sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights.
Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I
Parameters: X An arraylike object of the means of length n
Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2])
P : scalar, or np.array
Covariance of the filter. If scalar, is treated as eye(n)*P.
Returns: sigmas : np.array, of size (n, n+1)
Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space.
Ordered by Xi_0, Xi_{1..n}
