SquareRootKalmanFilter¶
Introduction and Overview¶
This implements a square root Kalman filter. No real attempt has been made to make this fast; it is a pedalogical exercise. The idea is that by computing and storing the square root of the covariance matrix we get about double the significant number of bits. Some authors consider this somewhat unnecessary with modern hardware. Of course, with microcontrollers being all the rage these days, that calculus has changed. But, will you really run a Kalman filter in Python on a tiny chip? No. So, this is for learning.
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.
Square Root Kalman Filter
-
class
filterpy.kalman.
SquareRootKalmanFilter
(dim_x, dim_z, dim_u=0)[source]¶ Create a Kalman filter which uses a square root implementation. This uses the square root of the state covariance matrix, which doubles the numerical precision of the filter, Therebuy reducing the effect of round off errors.
It is likely that you do not need to use this algorithm; we understand divergence issues very well now. However, if you expect the covariance matrix P to vary by 20 or more orders of magnitude then perhaps this will be useful to you, as the square root will vary by 10 orders of magnitude. From my point of view this is merely a ‘reference’ algorithm; I have not used this code in real world software. Brown[1] has a useful discussion of when you might need to use the square root form of this algorithm.
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.
References
- [1] Robert Grover Brown. Introduction to Random Signals and Applied
- Kalman Filtering. Wiley and sons, 2012.
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
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)covariance matrix of the prior
- x_post : numpy.array(dim_x, 1)
Posterior (updated) state estimate. Read Only.
P_post
: numpy.array(dim_x, dim_x)covariance matrix of the posterior
- z : numpy.array
Last measurement used in update(). Read only.
R
: numpy.array(dim_z, dim_z)measurement uncertainty
Q
: numpy.array(dim_x, dim_x)Process uncertainty
- F : numpy.array()
State Transition matrix
- H : numpy.array(dim_z, 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.
-
__init__
(dim_x, dim_z, dim_u=0)[source]¶ x.__init__(…) initializes x; see help(type(x)) for signature
-
update
(z, R2=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.
- R2 : np.array, scalar, or None
Sqrt of meaaurement noize. Optionally provide to override the measurement noise for this one call, otherwise self.R2 will be used.
-
predict
(u=0)[source]¶ Predict next state (prior) using the Kalman filter state propagation equations.
Parameters: - u : np.array, optional
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.
-
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
-
Q1_2
¶ Sqrt Process uncertainty
-
Q
¶ Process uncertainty
-
P_prior
¶ covariance matrix of the prior
-
P_post
¶ covariance matrix of the posterior
-
P1_2
¶ sqrt of covariance matrix
-
P
¶ covariance matrix
-
R1_2
¶ sqrt of measurement uncertainty
-
R
¶ measurement uncertainty