UnscentedKalmanFilter

Introduction and Overview

This implements the unscented Kalman filter. Still a work in development.

Copyright 2014 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.


class filterpy.kalman.UnscentedKalmanFilter(dim_x, dim_z, dt, hx, fx, kappa=0.0)[source]

Implements the Unscented Kalman filter (UKF) as defined by Simon J. Julier and Jeffery K. Uhlmann [1]. Succintly, the UKF selects a set of sigma points and weights inside the covariance matrix of the filter’s state. These points are transformed through the nonlinear process being filtered, and are rebuilt into a mean and covariance by computed the weighted mean and expected value of the transformed points. Read the paper; it is excellent. My book “Kalman and Bayesian Filters in Python” [2] explains the algorithm, develops this code, and provides examples of the filter in use.

You will have to set the following attributes after constructing this object for the filter to perform properly.

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.

Readable Attributes

Pxz : numpy.aray(dim_x, dim_z)
Cross variance of x and z computed during update() call.

References

[1]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)
[2]

Labbe, Roger R. “Kalman and Bayesian Filters in Python”

https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

__init__(dim_x, dim_z, dt, hx, fx, kappa=0.0)

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.
kappa : float, default=0.
Scaling factor that can reduce high order errors. kappa=0 gives the standard unscented filter. According to [1], if you set kappa to 3-dim_x for a Gaussian x you will minimize the fourth order errors in x and P.

References

[1] S. Julier, J. Uhlmann, and H. Durrant-Whyte. “A new method for
the nonlinear transformation of means and covariances in filters and estimators,” IEEE Transactions on Automatic Control, 45(3), pp. 477-482 (March 2000).
batch_filter(zs, Rs=None, residual=<Mock name='mock.subtract' id='4132147340'>, UT=None)

Performs the UKF filter over the list of measurement in zs.

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.
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)
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, but if for example you are using angles the default method of computing means and residuals will not work, and you will have to define how to compute it.

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.
predict(dt=None)

Performs the predict step of the UKF. On return, self.xp and self.Pp contain the predicted state (xp) and covariance (Pp). ‘p’ stands for prediction.

Parameters dt : double, optional

If specified, the time step to be used for this prediction. self._dt is used if this is not provided.

Important: this MUST be called before update() is called for the first time.

rts_smoother(Xs, Ps, Qs=None, dt=None)

Runs the Rauch-Tung-Striebal 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.
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
dt : optional, float or array-like 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

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)
static sigma_points(x, P, kappa)

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 An array-like 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. Each column contains all of the sigmas for one dimension in the problem space. They are ordered as:

\[sigmas[0] = x \]\[sigmas[1..n] = x + [\sqrt{(n+\kappa)P}]_k \]\[sigmas[n+1..2n] = x - [\sqrt{(n+\kappa)P}]_k\]
update(z, R=None, residual=<Mock name='mock.subtract' id='4132147340'>, UT=None)

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.
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)
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, but if for example you are using angles the default method of computing means and residuals will not work, and you will have to define how to compute it.
static weights(n, kappa)

Computes the weights for an unscented Kalman filter. See __init__() for meaning of parameters.


class filterpy.kalman.ScaledUnscentedKalmanFilter(dim_x, dim_z, dt, alpha, hx, fx, beta, kappa=0.0)[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.

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.

Readable Attributes

xp : numpy.array(dim_x)
predicted state (result of predict())
Pp : numpy.array(dim_x, dim_x)
predicted covariance matrix (result of predict())

References

[1]

Julier, Simon J. “The scaled unscented transformation,” American Control Converence, 2002, pp 4555-4559, vol 6.

Online copy: https://www.cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF

[2]

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

__init__(dim_x, dim_z, dt, alpha, hx, fx, beta, kappa=0.0)

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.
alpha : float
Determins the spread of the sigma points around the mean. Usually a small positive value (1e-3) 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 3-n according to [5].

References

[3]S. Julier, J. Uhlmann, and H. Durrant-Whyte. “A new method for the nonlinear transformation of means and covariances in filters and estimators,” IEEE Transactions on Automatic Control, 45(3), pp. 477-482 (March 2000).
[4]

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

[5]Wan, Merle “The Unscented Kalman Filter,” chapter in Kalman Filtering and Neural Networks, John Wiley & Sons, Inc., 2001.
predict()

Performs the predict step of the UKF. On return, self.xp and self.Pp contain the predicted state (xp) and covariance (Pp). ‘p’ stands for prediction.

Important: this MUST be called before update() is called for the first time.

static sigma_points(x, P, alpha, kappa)

Computes the sigma pointsfor an unscented Kalman filter given the mean (x) and covariance(P) of the filter. kappa is an arbitrary constant constant. 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 array-like 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.
alpha : float
Determines the spread of the sigma points around the mean.
kappa : float
Scaling factor.

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}

update(z, residual=<Mock name='mock.subtract' id='4132147340'>, UT=None)

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
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)
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, but if for example you are using angles the default method of computing means and residuals will not work, and you will have to define how to compute it.
static weights(n, alpha, beta, kappa)

Computes the weights for the scaled unscented Kalman filter.


filterpy.kalman.unscented_transform(Sigmas, Wm, Wc, noise_cov)[source]

Computes unscented transform of a set of sigma points and weights. returns the mean and covariance in a tuple.