From 77cf864d238315695b5f2e602847af51005d87f3 Mon Sep 17 00:00:00 2001
From: Roger Labbe <rlabbejr@gmail.com>
Date: Sun, 26 Jun 2016 13:32:01 -0700
Subject: [PATCH] Added K and y to UKF self

Allow external users to see the kalman gain and residual
---
 filterpy/kalman/UKF.py            |  9 +++---
 filterpy/kalman/tests/test_ukf.py | 47 +++++++++++++++++++++++++++++--
 2 files changed, 50 insertions(+), 6 deletions(-)

diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py
index 47acdc8..89e0d89 100644
--- a/filterpy/kalman/UKF.py
+++ b/filterpy/kalman/UKF.py
@@ -336,10 +336,11 @@ 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)
 
-        K = dot(Pxz, inv(Pz))   # Kalman gain
-        y = self.residual_z(z, zp)   #residual
-        self.x = self.x + dot(K, y)
-        self.P = self.P - dot3(K, Pz, K.T)
+        self.K = dot(Pxz, inv(Pz))        # Kalman gain
+        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)
 
 
     def batch_filter(self, zs, Rs=None, residual=None, UT=None):
diff --git a/filterpy/kalman/tests/test_ukf.py b/filterpy/kalman/tests/test_ukf.py
index 4233602..6ca1621 100644
--- a/filterpy/kalman/tests/test_ukf.py
+++ b/filterpy/kalman/tests/test_ukf.py
@@ -235,6 +235,49 @@ def hx(x):
         print(smooth_x)
 
 
+
+def test_linear_1d():
+    """ should work like a linear KF if problem is linear """
+
+
+    def fx(x, dt):
+        F = np.array([[1., dt],
+                      [0,  1]], dtype=float)
+
+        return np.dot(F, x)
+
+    def hx(x):
+        return np.array([x[0]])
+
+
+    dt = 0.1
+    points = MerweScaledSigmaPoints(2, .1, 2., -1)
+    kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
+
+
+    kf.x = np.array([1, 2])
+    kf.P = np.array([[1, 1.1],
+                     [1.1, 3]])
+    kf.R *= 0.05
+    kf.Q = np.array([[0., 0], [0., .001]])
+
+    z = np.array([2.])
+    kf.predict()
+    kf.update(z)
+
+    zs = []
+    for i in range(50):
+        z = np.array([i+randn()*0.1])
+        zs.append(z)
+
+        kf.predict()
+        kf.update(z)
+        print('K', kf.K.T)
+        print('x', kf.x)
+
+
+
+
 def test_batch_missing_data():
     """ batch filter should accept missing data with None in the measurements """
 
@@ -746,8 +789,8 @@ def fx(x, dt):
 
     DO_PLOT = True
 
-
-    test_batch_missing_data()
+    test_linear_1d()
+    #test_batch_missing_data()
 
     #test_linear_2d()
     #test_sigma_points_1D()