ExtendedKalmanFilter¶
Introduction and Overview¶
Implements a extended Kalman filter. For now the best documentation is my free book Kalman and Bayesian Filters in Python [1]
The test files in this directory also give you a basic idea of use, albeit without much description.
In brief, you will first construct this object, specifying the size of the state vector with dim_x and the size of the measurement vector that you will be using with dim_z. These are mostly used to perform size checks when you assign values to the various matrices. For example, if you specified dim_z=2 and then try to assign a 3x3 matrix to R (the measurement noise matrix you will get an assert exception because R should be 2x2. (If for whatever reason you need to alter the size of things midstream just use the underscore version of the matrices to assign directly: your_filter._R = a_3x3_matrix.)
After construction the filter will have default matrices created for you, but you must specify the values for each. It’s usually easiest to just overwrite them rather than assign to each element yourself. This will be clearer in the example below. All are of type numpy.array.
References
[1] | Labbe, Roger. “Kalman and Bayesian Filters in Python”. |
- github repo:
- https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
- read online:
- http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb
- PDF version (often lags the two sources above)
- https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/Kalman_and_Bayesian_Filters_in_Python.pdf
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.
-
class
filterpy.kalman.
ExtendedKalmanFilter
(dim_x, dim_z, dim_u=0)[source]¶ Implements an extended Kalman filter (EKF). You are responsible for setting the various state variables to reasonable values; the defaults will not give you a functional filter.
You will have to set the following attributes after constructing this object for the filter to perform properly. Please note that there are various checks in place to ensure that you have made everything the ‘correct’ size. However, it is possible to provide incorrectly sized arrays such that the linear algebra can not perform an operation. It can also fail silently - you can end up with matrices of a size that allows the linear algebra to work, but are the wrong shape for the problem you are trying to solve.
Parameters: - dim_x : int
Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4.
This is used to set the default size of P, Q, and u
- 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.
Examples
See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
Attributes: - x : numpy.array(dim_x, 1)
State estimate vector
- P : numpy.array(dim_x, dim_x)
Covariance matrix
- x_prior : numpy.array(dim_x, 1)
Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only.
- P_prior : numpy.array(dim_x, dim_x)
Prior (predicted) state covariance matrix. Read Only.
- x_post : numpy.array(dim_x, 1)
Posterior (updated) state estimate. Read Only.
- P_post : numpy.array(dim_x, dim_x)
Posterior (updated) state covariance matrix. Read Only.
- R : numpy.array(dim_z, dim_z)
Measurement noise matrix
- Q : numpy.array(dim_x, dim_x)
Process noise matrix
- F : numpy.array()
State Transition matrix
- H : numpy.array(dim_x, dim_x)
Measurement function
- y : numpy.array
Residual of the update step. Read only.
- K : numpy.array(dim_x, dim_z)
Kalman gain of the update step. Read only.
- S : numpy.array
Systen uncertaintly projected to measurement space. Read only.
- z : ndarray
Last measurement used in update(). Read only.
log_likelihood
: floatlog-likelihood of the last measurement.
likelihood
: floatComputed from the log-likelihood.
mahalanobis
: floatMahalanobis distance of innovation.
-
__init__
(dim_x, dim_z, dim_u=0)[source]¶ x.__init__(…) initializes x; see help(type(x)) for signature
-
predict_update
(z, HJacobian, Hx, args=(), hx_args=(), u=0)[source]¶ Performs the predict/update innovation of the extended Kalman filter.
Parameters: - z : np.array
measurement for this step. If None, only predict step is perfomed.
- HJacobian : function
function which computes the Jacobian of the H matrix (measurement function). Takes state variable (self.x) as input, along with the optional arguments in args, and returns H.
- Hx : function
function which takes as input the state variable (self.x) along with the optional arguments in hx_args, and returns the measurement that would correspond to that state.
- args : tuple, optional, default (,)
arguments to be passed into HJacobian after the required state variable.
- hx_args : tuple, optional, default (,)
arguments to be passed into Hx after the required state variable.
- u : np.array or scalar
optional control vector input to the filter.
-
update
(z, HJacobian, Hx, R=None, args=(), hx_args=(), residual=<Mock name='mock.subtract' id='139817310308176'>)[source]¶ Performs the update innovation of the extended Kalman filter.
Parameters: - z : np.array
measurement for this step. If None, posterior is not computed
- HJacobian : function
function which computes the Jacobian of the H matrix (measurement function). Takes state variable (self.x) as input, returns H.
- Hx : function
function which takes as input the state variable (self.x) along with the optional arguments in hx_args, and returns the measurement that would correspond to that state.
- R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used.
- args : tuple, optional, default (,)
arguments to be passed into HJacobian after the required state variable. for robot localization you might need to pass in information about the map and time of day, so you might have args=(map_data, time), where the signature of HCacobian will be def HJacobian(x, map, t)
- hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state variable.
- 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)
-
predict_x
(u=0)[source]¶ Predicts the next state of X. If you need to compute the next state yourself, override this function. You would need to do this, for example, if the usual Taylor expansion to generate F is not providing accurate results for you.
-
predict
(u=0)[source]¶ Predict next state (prior) using the Kalman filter state propagation equations.
Parameters: - u : np.array
Optional control vector. If non-zero, it is multiplied by B to create the control input into the system.
-
log_likelihood
¶ log-likelihood of the last measurement.
-
likelihood
¶ Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min.
-
mahalanobis
¶ Mahalanobis distance of innovation. E.g. 3 means measurement was 3 standard deviations away from the predicted value.
Returns: - mahalanobis : float