Issue rlabbe#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

@@ -516,15 +516,15 @@ 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
             Pxb = 0
             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))

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 """