class Test_1x1(unittest.TestCase): A = properties.ClassProperty('A') def test_assign_0D(self): self.A = properties.MatrixTemplate(1,1) self.A = 1234 self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 1234) def test_assign_1D(self): self.A = properties.MatrixTemplate(1,1) # vector (1D) matrx self.A = [3256] self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 3256) def test_assign_2D(self): self.A = properties.MatrixTemplate(1,1) # matrix 1x1, 2D array-like self.A = [[2394876]] self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 2394876) def test_assign_3D(self): self.A = properties.MatrixTemplate(1,1) # 3D array-like self.A = [[[32786]]] self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 32786)
class TestMatrix(unittest.TestCase): A = properties.ClassProperty('A') def test_invalid(self): m,n = 3,5 self.A = properties.MatrixTemplate(m,n) with self.assertRaises(ValueError): # A 1D vector whose number of elements is the same as that of the matrix self.A = np.zeros((m*n)) with self.assertRaises(ValueError): # Transposed will not be matched automatically of both dimensions are greater than 1 self.A = np.zeros((n,m)) for shape in [(n,), (m,), (m,1), (1,m), (n,1), (1,n)]: with self.assertRaises(ValueError): self.A = np.zeros(shape) self.A = np.zeros((m,n)) def test_multiplicative(self): self.A = properties.MatrixTemplate(4,4,multiplicative=True) self.A = 4 self.assertTrue(np.allclose(self.A, 4 * np.eye(4))) with self.assertRaises(ValueError): self.A = np.ones(4) self.A = properties.MatrixTemplate(4,4,multiplicative=False) with self.assertRaises(ValueError): self.A = 4 with self.assertRaises(ValueError): self.A = np.ones(4)
class TestZero(unittest.TestCase): A = properties.ClassProperty('A') F = properties.ClassProperty('F') def test_zero_matrix(self): for shape in [(1,4), (5, 1), (3,6), (7,3)]: self.A = properties.MatrixTemplate(*shape) self.A = 0 self.assertTrue(np.all(self.A == 0)) self.assertEqual(self.A.shape, shape) def test_zero_matrix_function(self): def zero(): return 0 for shape in [(1,4), (5, 1), (3,6), (7,3)]: self.F = properties.MatrixFunctionTemplate(*shape) self.F = zero self.assertTrue(np.all(self.F() == 0)) self.assertEqual(self.F().shape, shape)
class AutomaticConversionOnAssignment(unittest.TestCase): n = properties.ClassProperty('n'); def test_transformation(self): class EnsureEvenNumber(properties.Template): def __call__(self, v): return v - (v % 2) self.n = EnsureEvenNumber() self.n = 20; self.assertEqual(self.n, 20) self.n = 11 self.assertEqual(self.n, 10)
class PropertyValidation(unittest.TestCase): n = properties.ClassProperty('n') def test_validation(self): class ExpectEvenNumber(properties.Template): def __call__(self, v): assert (v % 2 == 0) return v self.n = ExpectEvenNumber() self.n = 4 self.n = 8 with self.assertRaises(Exception): self.n = 3
class TestVector(unittest.TestCase): A = properties.ClassProperty('A') def test_valid_multi_dimensional(self): for n in range(2, 5): x = list(range(n)) for shape in [(1,n), (n, 1)]: self.A = properties.MatrixTemplate(*shape) for d1 in range(3): x = [xi for xi in x]; y = x for d2 in range(3): y = [y] self.A = y self.assertEqual(self.A.shape, shape); def test_invalid_1D(self): for n in range(2, 5): for x in [list(range(1, n+2)), list(range(1, n))]: for shape in [(1,n), (n, 1)]: ### CHANGE THE TEMPLATE HERE ### self.A = properties.MatrixTemplate(*shape) for d1 in range(3): x = [xi for xi in x]; y = x for d2 in range(3): y = [y] with self.assertRaises(ValueError): self.A = y def test_invalid_interpretations_from_2D(self): m,n=3,5 for shape in [(m*n,1), (1,m*n)]: self.A = properties.MatrixTemplate(*shape) with self.assertRaises(ValueError): # Number of elements of the whole matrix match the size of the vector self.A = np.zeros((m,n)) with self.assertRaises(ValueError): # Columns of the matrix match the vector shape, but not the matrix self.A = np.zeros((m*n, 2)) with self.assertRaises(ValueError): # Rows of the matrix match the vector shape, but not the matrix self.A = np.zeros((2, m*n))
class ExtendedKalmanFilter(object): """ 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. 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 : float log-likelihood of the last measurement. Read only. likelihood : float likelihood of last measurment. Read only. 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 : float mahalanobis distance of the innovation. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Read only. Examples -------- See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python """ # overridable parameters guarded by checks x = properties.ClassProperty('x') P = properties.ClassProperty('P') Q = properties.ClassProperty('Q') B = properties.ClassProperty('B') F = properties.ClassProperty('F') R = properties.ClassProperty('R') z = properties.ClassProperty('z') def __init__(self, dim_x, dim_z, dim_u=0): self.dim_x = dim_x self.dim_z = dim_z self.dim_u = dim_u # Set templates that will hook the assignments and cast to # the specific shape self.x = properties.MatrixTemplate(dim_x, 1) self.P = properties.MatrixTemplate(dim_x, dim_x) self.Q = properties.MatrixTemplate(dim_x, dim_x) self.B = properties.MatrixTemplate(dim_x, dim_u) self.F = properties.MatrixTemplate(dim_x, dim_x) self.R = properties.MatrixTemplate(dim_z, dim_z) self.z = properties.MatrixTemplate(dim_z, 1) self.x = zeros((dim_x, 1)) # state self.P = eye(dim_x) # uncertainty covariance self.B = 0 # control transition matrix self.F = np.eye(dim_x) # state transition matrix self.R = eye(dim_z) # state uncertainty self.Q = eye(dim_x) # process uncertainty self.y = zeros((dim_z, 1)) # residual z = np.array([None]*self.dim_z) self.z = reshape_z(z, self.dim_z, self.x.ndim) # gain and residual are computed during the innovation step. We # save them so that in case you want to inspect them for various # purposes self.K = np.zeros(self.x.shape) # kalman gain self.y = zeros((dim_z, 1)) self.S = np.zeros((dim_z, dim_z)) # system uncertainty self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty # identity matrix. Do not alter this. self._I = np.eye(dim_x) self._log_likelihood = log(sys.float_info.min) self._likelihood = sys.float_info.min self._mahalanobis = None # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def predict_update(self, z, HJacobian, Hx, args=(), hx_args=(), u=0): """ 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. """ #pylint: disable=too-many-locals if not isinstance(args, tuple): args = (args,) if not isinstance(hx_args, tuple): hx_args = (hx_args,) if np.isscalar(z) and self.dim_z == 1: z = np.asarray([z], float) F = self.F B = self.B P = self.P Q = self.Q R = self.R x = self.x H = HJacobian(x, *args) # predict step x = dot(F, x) + dot(B, u) P = dot(F, P).dot(F.T) + Q # save prior self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) # update step PHT = dot(P, H.T) self.S = dot(H, PHT) + R self.SI = linalg.inv(self.S) self.K = dot(PHT, self.SI) self.y = z - Hx(x, *hx_args) self.x = x + dot(self.K, self.y) I_KH = self._I - dot(self.K, H) self.P = dot(I_KH, P).dot(I_KH.T) + dot(self.K, R).dot(self.K.T) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None def update(self, z, HJacobian, Hx, R=None, args=(), hx_args=(), residual=np.subtract): """ 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) """ if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() return if not isinstance(args, tuple): args = (args,) if not isinstance(hx_args, tuple): hx_args = (hx_args,) if R is None: R = self.R elif np.isscalar(R): R = eye(self.dim_z) * R if np.isscalar(z) and self.dim_z == 1: z = np.asarray([z], float) H = HJacobian(self.x, *args) PHT = dot(self.P, H.T) self.S = dot(H, PHT) + R self.SI = linalg.inv(self.S) self.K = PHT.dot(self.SI) hx = Hx(self.x, *hx_args) self.y = residual(z, hx) self.x = self.x + dot(self.K, self.y) # P = (I-KH)P(I-KH)' + KRK' is more numerically stable # and works for non-optimal K vs the equation # P = (I-KH)P usually seen in the literature. I_KH = self._I - dot(self.K, H) self.P = dot(I_KH, self.P).dot(I_KH.T) + dot(self.K, R).dot(self.K.T) # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() def predict_x(self, u=0): """ 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. """ u = properties.as_matrix((self.dim_u, 1), u) x = dot(self.F, self.x) + dot(self.B, u) self.x = x def predict(self, u=0): """ 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. """ self.predict_x(u) self.P = dot(self.F, self.P).dot(self.F.T) + self.Q # save prior self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) @property def log_likelihood(self): """ log-likelihood of the last measurement. """ if self._log_likelihood is None: self._log_likelihood = logpdf(x=self.y, cov=self.S) return self._log_likelihood @property def likelihood(self): """ 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. """ if self._likelihood is None: self._likelihood = exp(self.log_likelihood) if self._likelihood == 0: self._likelihood = sys.float_info.min return self._likelihood @property def mahalanobis(self): """ Mahalanobis distance of innovation. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Returns ------- mahalanobis : float """ if self._mahalanobis is None: self._mahalanobis = sqrt(float(dot(dot(self.y.T, self.SI), self.y))) return self._mahalanobis def __repr__(self): return '\n'.join([ 'KalmanFilter object', pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('x_prior', self.x_prior), pretty_str('P_prior', self.P_prior), pretty_str('F', self.F), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('S', self.S), pretty_str('likelihood', self.likelihood), pretty_str('log-likelihood', self.log_likelihood), pretty_str('mahalanobis', self.mahalanobis) ])
class TestMatrixFunctionProperty(unittest.TestCase): F = properties.ClassProperty('F') def test_decorated_function(self): for shape in [(3,5), (7,4)]: @properties.MatrixFunction(*shape) def f(x): return np.zeros(x) # check that a matrix was returned self.assertEqual(f(shape).shape, shape) with self.assertRaises(ValueError): f(shape[::-1]) # Tell F to return a matrix with wrog shape def test_decorated_function_assignment(self): shape = (3,7) @properties.MatrixFunction(*shape) def f(): raise TypeError("This should not be called") self.F = properties.MatrixFunctionTemplate(*shape) self.F = f # matches the template self.F = properties.MatrixFunctionTemplate(*shape[::-1]) with self.assertRaises(ValueError): self.F = f # does not match the template def test_undecorated_function_assignment(self): shape = (3,7) def f(): return 1 self.F = properties.MatrixFunctionTemplate(*shape) # since f was not decorated it will accept expecting it # to return matrices of the given shape self.F = f with self.assertRaises(ValueError): self.F() # when f is called it will see an invalid value def test_multiplicativify_of_function(self): # Non multiplicative template self.F = properties.MatrixFunctionTemplate(4,4,multiplicative=False) def identity(x): return x # Undecorated function self.F = identity with self.assertRaises(ValueError): self.F(4) self.assertTrue(np.allclose(self.F(4*np.eye(4)), 4*np.eye(4))) # Non multiplicative template multiplicative function self.F = properties.DecoratedMatrixFunction((4,4), identity, multiplicative=True) self.assertTrue(np.allclose(self.F(4*np.eye(4)), 4*np.eye(4))) # Multiplicative template self.F = properties.MatrixFunctionTemplate(4,4,multiplicative=False) # Multiplicative template undecorated function self.F = identity self.assertTrue(np.allclose(self.F(4*np.eye(4)), 4*np.eye(4))) # Multiplicative template multiplicative function self.F = properties.DecoratedMatrixFunction((4,4), identity, multiplicative=True) self.assertTrue(np.allclose(self.F(4*np.eye(4)), 4*np.eye(4))) # Multiplicative template non-multiplicative function self.F = properties.DecoratedMatrixFunction((4,4), identity, multiplicative=False) with self.assertRaises(ValueError): self.F(6)
class Class: prop1 = properties.ClassProperty('prop1') prop2 = properties.ClassProperty('prop2')
class UnscentedKalmanFilter(object): # pylint: disable=too-many-instance-attributes # pylint: disable=invalid-name r""" 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. 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. This is for convience, so everything is sized correctly on creation. If you are using multiple sensors the size of `z` can change based on the sensor. Just provide the appropriate hx function dt : float Time between steps in seconds. hx : function(x,**hx_args) Measurement function. Converts state vector x into a measurement vector of shape (dim_z). fx : function(x,dt,**fx_args) function that returns the state x transformed by the state transition function. dt is the time step in seconds. points : class Class which computes the sigma points and weights for a UKF algorithm. You can vary the UKF implementation by changing this class. For example, MerweScaledSigmaPoints implements the alpha, beta, kappa parameterization of Van der Merwe, and JulierSigmaPoints implements Julier's original kappa parameterization. See either of those for the required signature of this class if you want to implement your own. sqrt_fn : callable(ndarray), default=None (implies scipy.linalg.cholesky) Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe's dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you. If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing as far as this class is concerned. x_mean_fn : callable (sigma_points, weights), optional Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed. .. code-block:: Python def state_mean(sigmas, Wm): x = np.zeros(3) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += sin(s[2])*Wm[i] sum_cos += cos(s[2])*Wm[i] x[2] = atan2(sum_sin, sum_cos) return x z_mean_fn : callable (sigma_points, weights), optional Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx(). residual_x : callable (x, y), optional residual_z : callable (x, y), optional Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. One is for the state variable, the other is for the measurement state. .. code-block:: Python def residual(a, b): y = a[0] - b[0] if y > np.pi: y -= 2*np.pi if y < -np.pi: y += 2*np.pi return y state_add: callable (x, y), optional, default np.add Function that subtracts two state vectors, returning a new state vector. Used during update to compute `x + K@y` You will have to supply this if your state variable does not suport addition, such as it contains angles. Attributes ---------- x : numpy.array(dim_x) state estimate vector P : numpy.array(dim_x, dim_x) covariance estimate matrix x_prior : numpy.array(dim_x) 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) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. z : ndarray Last measurement used in update(). Read only. R : numpy.array(dim_z, dim_z) measurement noise matrix Q : numpy.array(dim_x, dim_x) process noise matrix K : numpy.array Kalman gain y : numpy.array innovation residual log_likelihood : scalar Log likelihood of last measurement update. likelihood : float likelihood of last measurment. Read only. 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 : float mahalanobis distance of the measurement. Read only. inv : function, default numpy.linalg.inv If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead: .. code-block:: Python kf.inv = np.linalg.pinv Examples -------- Simple example of a linear order 1 kinematic filter in 2D. There is no need to use a UKF for this example, but it is easy to read. >>> def fx(x, dt): >>> # state transition function - predict next state based >>> # on constant velocity model x = vt + x_0 >>> F = np.array([[1, dt, 0, 0], >>> [0, 1, 0, 0], >>> [0, 0, 1, dt], >>> [0, 0, 0, 1]], dtype=float) >>> return np.dot(F, x) >>> >>> def hx(x): >>> # measurement function - convert state into a measurement >>> # where measurements are [x_pos, y_pos] >>> return np.array([x[0], x[2]]) >>> >>> dt = 0.1 >>> # create sigma points to use in the filter. This is standard for Gaussian processes >>> points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1) >>> >>> kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) >>> kf.x = np.array([-1., 1., -1., 1]) # initial state >>> kf.P *= 0.2 # initial uncertainty >>> z_std = 0.1 >>> kf.R = np.diag([z_std**2, z_std**2]) # 1 standard >>> kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2) >>> >>> zs = [[i+randn()*z_std, i+randn()*z_std] for i in range(50)] # measurements >>> for z in zs: >>> kf.predict() >>> kf.update(z) >>> print(kf.x, 'log-likelihood', kf.log_likelihood) For in depth explanations see my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python Also see the filterpy/kalman/tests subdirectory for test code that may be illuminating. 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 .. [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. .. [6] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation) """ # overridable parameters guarded by checks x = properties.ClassProperty('x') P = properties.ClassProperty('P') Q = properties.ClassProperty('Q') F = properties.ClassProperty('F') H = properties.ClassProperty('H') R = properties.ClassProperty('R') M = properties.ClassProperty('M') z = properties.ClassProperty('z') def __init__(self, dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None, state_add=None): """ 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. """ # Set templates that will hook the assignments and cast to # the specific shape self.x = properties.MatrixTemplate(dim_x, 1) self.P = properties.MatrixTemplate(dim_x, dim_x) self.Q = properties.MatrixTemplate(dim_x, dim_x) self.F = properties.MatrixFunctionTemplate(dim_x, dim_x) self.H = properties.MatrixFunctionTemplate(dim_z, dim_x) self.R = properties.MatrixTemplate(dim_z, dim_z) self.M = properties.MatrixTemplate(dim_x, dim_z) self.z = properties.MatrixTemplate(dim_z, 1) #pylint: disable=too-many-arguments self.x = zeros(dim_x) self.P = eye(dim_x) self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) self.Q = eye(dim_x) self.R = eye(dim_z) self._dim_x = dim_x self._dim_z = dim_z self.points_fn = points self._dt = dt self._num_sigmas = points.num_sigmas() self.hx = hx self.fx = fx self.x_mean = x_mean_fn self.z_mean = z_mean_fn # Only computed only if requested via property self._log_likelihood = log(sys.float_info.min) self._likelihood = sys.float_info.min self._mahalanobis = None if sqrt_fn is None: self.msqrt = cholesky else: self.msqrt = sqrt_fn # weights for the means and covariances. self.Wm, self.Wc = points.Wm, points.Wc if residual_x is None: self.residual_x = np.subtract else: self.residual_x = residual_x if residual_z is None: self.residual_z = np.subtract else: self.residual_z = residual_z if state_add is None: self.state_add = np.add else: self.state_add = state_add # sigma points transformed through f(x) and h(x) # variables for efficiency so we don't recreate every update self.sigmas_f = zeros((self._num_sigmas, self._dim_x)) self.sigmas_h = zeros((self._num_sigmas, self._dim_z)) self.K = np.zeros((dim_x, dim_z)) # Kalman gain self.y = np.zeros((dim_z)) # residual self.z = np.array([[None]*dim_z]).T # measurement self.S = np.zeros((dim_z, dim_z)) # system uncertainty self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty self.inv = np.linalg.inv # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def predict(self, dt=None, UT=None, fx=None, **fx_args): r""" Performs the predict step of the UKF. On return, self.x and self.P contain the predicted state (x) and covariance (P). ' Important: this MUST be called before update() is called for the first time. Parameters ---------- dt : double, optional If specified, the time step to be used for this prediction. self._dt is used if this is not provided. fx : callable f(x, dt, **fx_args), optional State transition function. If not provided, the default function passed in during construction will be used. 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 - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. **fx_args : keyword arguments optional keyword arguments to be passed into f(x). """ if dt is None: dt = self._dt if UT is None: UT = unscented_transform # calculate sigma points for given mean and covariance self.compute_process_sigmas(dt, fx, **fx_args) #and pass sigmas through the unscented transform to compute prior self.x, self.P = UT(self.sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x) # update sigma points to reflect the new variance of the points self.sigmas_f = self.points_fn.sigma_points(self.x, self.P) # save prior self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) def update(self, z, R=None, UT=None, hx=None, **hx_args): """ 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. 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 - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. hx : callable h(x, **hx_args), optional Measurement function. If not provided, the default function passed in during construction will be used. **hx_args : keyword argument arguments to be passed into h(x) after x -> h(x, **hx_args) """ if z is None: self.z = np.array([[None]*self._dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() return else: z = properties.as_matrix((self._dim_z, 1), z, 'z') if hx is None: hx = self.hx if UT is None: UT = unscented_transform if R is None: R = self.R elif isscalar(R): R = eye(self._dim_z) * R else: R = properties.as_matrix((self._dim_z,)*2, R, 'R') # pass prior sigmas through h(x) to get measurement sigmas # the shape of sigmas_h will vary if the shape of z varies, so # recreate each time sigmas_h = [hx(s, **hx_args) for s in self.sigmas_f] sigmas_h = np.reshape(sigmas_h, [-1, self._dim_z, 1]) # mean and covariance of prediction passed through unscented transform zp, self.S = UT(self.sigmas_h, self.Wm, self.Wc, R, self.z_mean, self.residual_z) self.SI = self.inv(self.S) # compute cross variance of the state and the measurements Pxz = self.cross_variance(self.x, zp, self.sigmas_f, self.sigmas_h) self.K = dot(Pxz, self.SI) # Kalman gain self.y = self.residual_z(z, zp) # residual # update Gaussian state estimate (x, P) self.x = self.state_add(self.x, dot(self.K, self.y)) self.P = self.P - dot(self.K, dot(self.S, self.K.T)) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None def cross_variance(self, x, z, sigmas_f, sigmas_h): """ Compute cross variance of the state `x` and measurement `z`. """ Pxz = zeros((sigmas_f.shape[1], sigmas_h.shape[1])) N = sigmas_f.shape[0] x = np.reshape(x, (-1,)) z = np.reshape(z, (-1,)) for i in range(N): dx = self.residual_x(sigmas_f[i], x) dz = self.residual_z(sigmas_h[i], z) Pxz += self.Wc[i] * outer(dx, dz) return Pxz def compute_process_sigmas(self, dt, fx=None, **fx_args): """ computes the values of sigmas_f. Normally a user would not call this, but it is useful if you need to call update more than once between calls to predict (to update for multiple simultaneous measurements), so the sigmas correctly reflect the updated state x, P. """ if fx is None: fx = self.fx # calculate sigma points for given mean and covariance sigmas = self.points_fn.sigma_points(self.x, self.P) for i, s in enumerate(sigmas): self.sigmas_f[i] = np.squeeze(fx(s, dt, **fx_args)) def batch_filter(self, zs, Rs=None, dts=None, UT=None, saver=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 : None, np.array or list-like, default=None optional list of values to use for the measurement error covariance R. If Rs is None then self.R is used for all epochs. If it is a list of matrices or a 3D array where len(Rs) == len(zs), then it is treated as a list of R values, one per epoch. This allows you to have varying R per epoch. dts : None, scalar or list-like, default=None optional value or list of delta time to be passed into predict. If dtss is None then self.dt is used for all epochs. If it is a list where len(dts) == len(zs), then it is treated as a list of dt values, one per epoch. This allows you to have varying epoch durations. 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 - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. saver : filterpy.common.Saver, optional filterpy.common.Saver object. If provided, saver.save() will be called after every epoch Returns ------- means: ndarray((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: ndarray((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`. Examples -------- .. code-block:: Python # this example demonstrates tracking a measurement where the time # between measurement varies, as stored in dts The output is then smoothed # with an RTS smoother. zs = [t + random.randn()*4 for t in range (40)] (mu, cov, _, _) = ukf.batch_filter(zs, dts=dts) (xs, Ps, Ks) = ukf.rts_smoother(mu, cov) """ #pylint: disable=too-many-arguments try: z = zs[0] except TypeError: raise TypeError('zs must be list-like') if self._dim_z == 1: if not(isscalar(z) or (z.ndim == 1 and len(z) == 1)): raise TypeError('zs must be a list of scalars or 1D, 1 element arrays') else: if len(z) != self._dim_z: raise TypeError( 'each element in zs must be a 1D array of length {}'.format(self._dim_z)) z_n = np.size(zs, 0) if Rs is None: Rs = [self.R] * z_n if dts is None: dts = [self._dt] * z_n # mean estimates from Kalman Filter if self.x.ndim == 1: means = zeros((z_n, self._dim_x)) else: means = zeros((z_n, self._dim_x, 1)) # state covariances from Kalman Filter covariances = zeros((z_n, self._dim_x, self._dim_x)) for i, (z, r, dt) in enumerate(zip(zs, Rs, dts)): self.predict(dt=dt, UT=UT) self.update(z, r, UT=UT) means[i, :] = self.x covariances[i, :, :] = self.P if saver is not None: saver.save() return (means, covariances) def rts_smoother(self, Xs, Ps, Qs=None, dts=None, UT=None): """ Runs the Rauch-Tung-Striebel 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. Qs: 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. 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 - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. Returns ------- x : numpy.ndarray smoothed means P : numpy.ndarray smoothed state covariances K : numpy.ndarray smoother gain at each step Examples -------- .. code-block:: Python 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) """ #pylint: disable=too-many-locals, too-many-arguments if len(Xs) != len(Ps): raise ValueError('Xs and Ps must have the same length') n, dim_x = Xs.shape if dts is None: dts = [self._dt] * n elif isscalar(dts): dts = [dts] * n if Qs is None: Qs = [self.Q] * n if UT is None: UT = unscented_transform # smoother gain Ks = zeros((n, dim_x, dim_x)) num_sigmas = self._num_sigmas xs, ps = Xs.copy(), Ps.copy() sigmas_f = zeros((num_sigmas, dim_x)) for k in reversed(range(n-1)): # create sigma points from state estimate, pass through state func sigmas = self.points_fn.sigma_points(xs[k], ps[k]) for i in range(num_sigmas): sigmas_f[i] = self.fx(sigmas[i], dts[k]) xb, Pb = UT( sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x) # compute cross variance Pxb = 0 for i in range(num_sigmas): y = self.residual_x(sigmas_f[i], xb) z = self.residual_x(sigmas[i], Xs[k]) Pxb += self.Wc[i] * outer(z, y) # compute gain K = dot(Pxb, self.inv(Pb)) # update the smoothed estimates xs[k] += dot(K, self.residual_x(xs[k+1], xb)) ps[k] += dot(K, ps[k+1] - Pb).dot(K.T) Ks[k] = K return (xs, ps, Ks) @property def log_likelihood(self): """ log-likelihood of the last measurement. """ if self._log_likelihood is None: self._log_likelihood = logpdf(x=self.y, cov=self.S) return self._log_likelihood @property def likelihood(self): """ 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. """ if self._likelihood is None: self._likelihood = exp(self.log_likelihood) if self._likelihood == 0: self._likelihood = sys.float_info.min return self._likelihood @property def mahalanobis(self): """" Mahalanobis distance of measurement. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Returns ------- mahalanobis : float """ if self._mahalanobis is None: self._mahalanobis = sqrt(float(dot(dot(self.y.T, self.SI), self.y))) return self._mahalanobis def __repr__(self): return '\n'.join([ 'UnscentedKalmanFilter object', pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('x_prior', self.x_prior), pretty_str('P_prior', self.P_prior), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('S', self.S), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('log-likelihood', self.log_likelihood), pretty_str('likelihood', self.likelihood), pretty_str('mahalanobis', self.mahalanobis), pretty_str('sigmas_f', self.sigmas_f), pretty_str('h', self.sigmas_h), pretty_str('Wm', self.Wm), pretty_str('Wc', self.Wc), pretty_str('residual_x', self.residual_x), pretty_str('residual_z', self.residual_z), pretty_str('msqrt', self.msqrt), pretty_str('hx', self.hx), pretty_str('fx', self.fx), pretty_str('x_mean', self.x_mean), pretty_str('z_mean', self.z_mean) ])