diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py index 41013e1..3c52e1c 100644 --- a/filterpy/kalman/UKF.py +++ b/filterpy/kalman/UKF.py @@ -19,10 +19,11 @@ from filterpy.common import dot3 from filterpy.kalman import unscented_transform +import math import numpy as np from numpy import eye, zeros, dot, isscalar, outer from scipy.linalg import inv, cholesky - +from scipy.stats import multivariate_normal class UnscentedKalmanFilter(object): # pylint: disable=too-many-instance-attributes @@ -56,12 +57,26 @@ class UnscentedKalmanFilter(object): Readable Attributes ------------------- + K : numpy.array Kalman gain y : numpy.array innovation residual + x : numpy.array(dim_x) + predicted/updated state (result of predict()/update()) + + P : numpy.array(dim_x, dim_x) + predicted/updated covariance matrix (result of predict()/update()) + + likelihood : scalar + Likelihood of last measurement update. + + log_likelihood : scalar + Log likelihood of last measurement update. + + References ---------- @@ -214,6 +229,7 @@ def residual(a, b): self.fx = fx self.x_mean = x_mean_fn self.z_mean = z_mean_fn + self.log_likelihood = 0.0 if sqrt_fn is None: self.msqrt = cholesky @@ -240,7 +256,6 @@ def residual(a, b): self.sigmas_h = zeros((self._num_sigmas, self._dim_z)) - def predict(self, dt=None, UT=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). ' @@ -337,12 +352,21 @@ def update(self, z, R=None, UT=None, hx_args=()): dz = self.residual_z(self.sigmas_h[i], zp) Pxz += self.Wc[i] * outer(dx, dz) + self.K = dot(Pxz, inv(Pz)) # Kalman gain - self.y = self.residual_z(z, zp) #residual + self.y = self.residual_z(z, zp) # residual self.x = self.x + dot(self.K, self.y) self.P = self.P - dot3(self.K, Pz, self.K.T) + self.log_likelihood = multivariate_normal.logpdf( + x=self.y, mean=np.zeros(len(self.y)), cov=Pz, allow_singular=True) + + + @property + def likelihood(self): + return math.exp(self.log_likelihood) + def batch_filter(self, zs, Rs=None, UT=None): """ Performs the UKF filter over the list of measurement in `zs`.