Kalman Filter¶
Implements a linear 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.
These are the matrices (instance variables) which you must specify. All are of type numpy.array (do NOT use numpy.matrix) If dimensional analysis allows you to get away with a 1x1 matrix you may also use a scalar. All elements must have a type of float.
Instance Variables
You will have to assign reasonable values to all of these before running the filter. All must have dtype of float.
- x : ndarray (dim_x, 1), default = [0,0,0...0]
- filter state estimate
- P : ndarray (dim_x, dim_x), default eye(dim_x)
- covariance matrix
- Q : ndarray (dim_x, dim_x), default eye(dim_x)
- Process uncertainty/noise
- R : ndarray (dim_z, dim_z), default eye(dim_x)
- measurement uncertainty/noise
- H : ndarray (dim_z, dim_x)
- measurement function
- F : ndarray (dim_x, dim_x)
- state transistion matrix
- B : ndarray (dim_x, dim_u), default 0
- control transition matrix
Optional Instance Variables
alpha : float
Assign a value > 1.0 to turn this into a fading memory filter.
Read-only Instance Variables
- K : ndarray
- Kalman gain that was used in the most recent update() call.
- y : ndarray
- Residual calculated in the most recent update() call. I.e., the different between the measurement and the current estimated state projected into measurement space (z - Hx)
- S : ndarray
- System uncertainty projected into measurement space. I.e., HPH’ + R. Probably not very useful, but it is here if you want it.
Example
Here is a filter that tracks position and velocity using a sensor that only reads position.
First construct the object with the required dimensionality.
f = KalmanFilter (dim_x=2, dim_z=1)
Assign the initial value for the state (position and velocity). You can do this with a two dimensional array like so:
f.x = np.array([[2.], # position
[0.]]) # velocity
or just use a one dimensional array, which I prefer doing.
f.x = np.array([2., 0.])
Define the state transition matrix:
f.F = np.array([[1.,1.],
[0.,1.]])
Define the measurement function:
f.H = np.array([[1.,0.]])
Define the covariance matrix. Here I take advantage of the fact that P already contains np.eye(dim_x), and just multipy by the uncertainty:
f.P *= 1000.
I could have written:
f.P = np.array([[1000., 0.],
[ 0., 1000.] ])
You decide which is more readable and understandable.
Now assign the measurement noise. Here the dimension is 1x1, so I can use a scalar
f.R = 5
I could have done this instead:
f.R = np.array([[5.]])
Note that this must be a 2 dimensional array, as must all the matrices.
Finally, I will assign the process noise. Here I will take advantage of another FilterPy library function:
from filterpy.common import Q_discrete_white_noise
f.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
Now just perform the standard predict/update loop:
while some_condition_is_true:
z = get_sensor_reading()
f.predict()
f.update(z)
do_something_with_estimate (f.x)
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 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.
Kalman filter
-
class
filterpy.kalman.
KalmanFilter
(dim_x, dim_z, dim_u=0)[source]¶ Implements a Kalman filter. 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.
Attributes
- x : numpy.array(dim_x, 1)
- 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
- F : numpy.array()
- State Transition matrix
H : numpy.array(dim_x, dim_x)
You may read the following attributes.
Readable Attributes
- y : numpy.array
- Residual of the update step.
- K : numpy.array(dim_x, dim_z)
- Kalman gain of the update step
- S : numpy.array
- Systen uncertaintly projected to measurement space
-
__init__
(dim_x, dim_z, dim_u=0)[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 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.
- dim_u : int (optional)
- size of the control input, if it is being used. Default value of 0 indicates it is not used.
-
B
¶ control transition matrix
-
F
¶ state transition matrix
-
K
¶ Kalman gain
-
P
¶ covariance matrix
-
Q
¶ Process uncertainty
-
S
¶ system uncertainty in measurement space
-
alpha
¶ Fading memory setting. 1.0 gives the normal Kalman filter, and values slightly larger than 1.0 (such as 1.02) give a fading memory effect - previous measurements have less influence on the filter’s estimates. This formulation of the Fading memory filter (there are many) is due to Dan Simon [1].
** References **
- [1] Dan Simon. “Optimal State Estimation.” John Wiley & Sons.
- 208-212. (2006)
-
batch_filter
(zs, Rs=None, update_first=False)[source]¶ Batch processes a sequences of measurements.
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.
- update_first : bool, optional,
- controls whether the order of operations is update followed by predict, or predict followed by update. Default is predict->update.
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.
- means_predictions: np.array((n,dim_x,1))
- array of the state for each time step after the predictions. Each entry is an np.array. In other words means[k,:] is the state at step k.
- covariance_predictions: np.array((n,dim_x,dim_x))
- array of the covariances for each time step after the prediction. In other words covariance[k,:,:] is the covariance at step k.
-
get_prediction
(u=0)[source]¶ Predicts the next state of the filter and returns it. Does not alter the state of the filter.
Parameters
- u : np.array
- optional control input
Returns
- (x, P)
- State vector and covariance array of the prediction.
-
measurement_of_state
(x)[source]¶ Helper function that converts a state into a measurement.
Parameters
- x : np.array
- kalman state vector
Returns
- z : np.array
- measurement corresponding to the given state
-
predict
(u=0)[source]¶ Predict next position.
Parameters
- u : np.array
- Optional control vector. If non-zero, it is multiplied by B to create the control input into the system.
-
residual_of
(z)[source]¶ returns the residual for the given measurement (z). Does not alter the state of the filter.
-
rts_smoother
(Xs, Ps, Qs=None)[source]¶ Runs the Rauch-Tung-Striebal Kalman smoother on a set of means and covariances computed by a Kalman filter. The usual input would come from the output of KalmanFilter.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
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)
-
test_matrix_dimensions
()[source]¶ Performs a series of asserts to check that the size of everything is what it should be. This can help you debug problems in your design.
This is only a test; you do not need to use it while filtering. However, to use you will want to perform at least one predict() and one update() before calling; some bad designs will cause the shapes of x and P to change in a silent and bad way. For example, if you pass in a badly dimensioned z into update that can cause x to be misshapen.
-
update
(z, R=None, H=None)[source]¶ Add a new measurement (z) to the kalman filter. If z is None, nothing is changed.
Parameters
- z : np.array
- measurement for this update.
- R : np.array, scalar, or None
- Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used.
-
x
¶ filter state vector.
-
y
¶ measurement residual (innovation)