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.