Merge branch 'williamkreamer-ukf-log-likelihood'

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`.