From 10f50dc29175cd7aa102bd16afceabc9e6c1accd Mon Sep 17 00:00:00 2001
From: Roger Labbe <rlabbejr@gmail.com>
Date: Tue, 20 Sep 2016 06:26:28 -0700
Subject: [PATCH] Issue #51. Wrong weights in UKF RTS smoother.

The code used the mean weights in the computation for the covariances,
when obviously the covariance weights should be used.
---
 filterpy/kalman/UKF.py            |  4 +--
 filterpy/kalman/tests/test_ukf.py | 51 +++++++++++++++++++++++++++----
 2 files changed, 47 insertions(+), 8 deletions(-)

diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py
index 3c52e1c..a43bbb1 100644
--- a/filterpy/kalman/UKF.py
+++ b/filterpy/kalman/UKF.py
@@ -516,7 +516,7 @@ def rts_smoother(self, Xs, Ps, Qs=None, dt=None):
             x = Xs[k]
             for i in range(num_sigmas):
                 y = sigmas_f[i] - x
-                Pb += self.Wm[i] * outer(y, y)
+                Pb += self.Wc[i] * outer(y, y)
             Pb += Qs[k]
 
             # compute cross variance
@@ -524,7 +524,7 @@ def rts_smoother(self, Xs, Ps, Qs=None, dt=None):
             for i in range(num_sigmas):
                 z = sigmas[i] - Xs[k]
                 y = sigmas_f[i] - xb
-                Pxb += self.Wm[i] * outer(z, y)
+                Pxb += self.Wc[i] * outer(z, y)
 
             # compute gain
             K = dot(Pxb, inv(Pb))
diff --git a/filterpy/kalman/tests/test_ukf.py b/filterpy/kalman/tests/test_ukf.py
index 85a7c24..0f69822 100644
--- a/filterpy/kalman/tests/test_ukf.py
+++ b/filterpy/kalman/tests/test_ukf.py
@@ -32,6 +32,8 @@
 from math import cos, sin
 
 
+
+
 DO_PLOT = False
 
 
@@ -240,7 +242,7 @@ def hx(x):
         plt.plot(t, xs[:,2])
 
 
-def test_linear_2d():
+def test_linear_2d_merwe():
     """ should work like a linear KF if problem is linear """
 
 
@@ -257,8 +259,7 @@ def hx(x):
 
 
     dt = 0.1
-    # points = MerweScaledSigmaPoints(4, .1, 2., -1)
-    points = SimplexSigmaPoints(n=4)
+    points = MerweScaledSigmaPoints(4, .1, 2., -1)
     kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
 
 
@@ -272,7 +273,48 @@ def hx(x):
         z = np.array([i+randn()*0.1, i+randn()*0.1])
         zs.append(z)
 
+    Ms, Ps = kf.batch_filter(zs)
+    smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
+
+    if DO_PLOT:
+        plt.figure()
+        zs = np.asarray(zs)
+        plt.plot(zs[:,0], marker='+')
+        plt.plot(Ms[:,0], c='b')
+        plt.plot(smooth_x[:,0], smooth_x[:,2], c='r')
+        print(smooth_x)
+
+
+def test_linear_2d_simplex():
+    """ should work like a linear KF if problem is linear """
+
+
+    def fx(x, dt):
+        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):
+        return np.array([x[0], x[2]])
+
+
+    dt = 0.1
+    points = SimplexSigmaPoints(n=4)
+    kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
+
+
+    kf.x = np.array([-1., 1., -1., 1])
+    kf.P*=0.0001
+    #kf.R *=0
+    #kf.Q
 
+    zs = []
+    for i in range(20):
+        z = np.array([i+randn()*0.1, i+randn()*0.1])
+        zs.append(z)
 
     Ms, Ps = kf.batch_filter(zs)
     smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
@@ -286,8 +328,6 @@ def hx(x):
 
         print(smooth_x)
 
-
-
 def test_linear_1d():
     """ should work like a linear KF if problem is linear """
 
@@ -329,7 +369,6 @@ def hx(x):
 
 
 
-
 def test_batch_missing_data():
     """ batch filter should accept missing data with None in the measurements """