Merge of simplex points.

I changed the implementation to not use strings to determine the size
of num_sigmas, but instead a function in the simga classes.

filterpy/kalman/UKF.py

@@ -209,10 +209,7 @@ def residual(a, b):
         self._dim_z = dim_z
         self.points_fn = points
         self._dt = dt
-        if self.points_fn.name == 'Simplex':
-            self._num_sigmas = dim_x + 1
-        else:
-            self._num_sigmas = 2*dim_x + 1
+        self._num_sigmas = points.num_sigmas()
         self.hx = hx
         self.fx = fx
         self.x_mean = x_mean_fn

filterpy/kalman/sigma_points.py

@@ -76,7 +76,6 @@ def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None):
         """
 
         self.n = n
-        self.name = 'Merwe'
         self.alpha = alpha
         self.beta = beta
         self.kappa = kappa
@@ -91,6 +90,11 @@ def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None):
             self.subtract = subtract
 
 
+    def num_sigmas(self):
+        """ Number of sigma points for each variable in the state x"""
+        return 2*self.n + 1
+
+
     def sigma_points(self, x, P):
         """ Computes the sigma points for an unscented Kalman filter
         given the mean (x) and covariance(P) of the filter.
@@ -219,7 +223,6 @@ def __init__(self,n,  kappa, sqrt_method=None, subtract=None):
        """
 
         self.n = n
-        self.name = 'Julier'
         self.kappa = kappa
         if sqrt_method is None:
             self.sqrt = cholesky
@@ -232,6 +235,11 @@ def __init__(self,n,  kappa, sqrt_method=None, subtract=None):
             self.subtract = subtract
 
 
+    def num_sigmas(self):
+        """ Number of sigma points for each variable in the state x"""
+        return 2*self.n + 1
+
+
     def sigma_points(self, x, P):
         r""" Computes the sigma points for an unscented Kalman filter
         given the mean (x) and covariance(P) of the filter.
@@ -321,8 +329,8 @@ class SimplexSigmaPoints(object):
 
     def __init__(self, n, alpha=1, sqrt_method=None, subtract=None):
         """
-        Generates sigma points and weights according to the simplex method presented in [1]
-         DOI: 10.1051/cocv/2010006
+        Generates sigma points and weights according to the simplex method
+        presented in [1] DOI: 10.1051/cocv/2010006
 
         Parameters
         ----------
@@ -350,12 +358,12 @@ def __init__(self, n, alpha=1, sqrt_method=None, subtract=None):
         References
         ----------
 
-        .. [1] Phillippe Moireau and Dominique Chapelle "Reduced-Order Unscented Kalman Filtering with Application to
-        Parameter Identification in Large-Dimensional Systems"
+        .. [1] Phillippe Moireau and Dominique Chapelle "Reduced-Order Unscented
+        Kalman Filtering with Application to Parameter Identification in
+        Large-Dimensional Systems"
         """
 
         self.n = n
-        self.name = 'Simplex'
         self.alpha = alpha
         if sqrt_method is None:
             self.sqrt = cholesky
@@ -368,6 +376,11 @@ def __init__(self, n, alpha=1, sqrt_method=None, subtract=None):
             self.subtract = subtract
 
 
+    def num_sigmas(self):
+        """ Number of sigma points for each variable in the state x"""
+        return self.n + 1
+
+
     def sigma_points(self, x, P):
         """ Computes the implex sigma points for an unscented Kalman filter
         given the mean (x) and covariance(P) of the filter.
@@ -412,7 +425,7 @@ def sigma_points(self, x, P):
 
         U = self.sqrt(P)
 
-        lambda_ = n/(n+1)
+        lambda_ = n / (n + 1)
         Istar = np.array([[-1/np.sqrt(2*lambda_), 1/np.sqrt(2*lambda_)]])
         for d in range(2, n+1):
             row = np.ones((1, Istar.shape[1] + 1)) * 1. / np.sqrt(lambda_*d*(d + 1))
@@ -422,9 +435,7 @@ def sigma_points(self, x, P):
         I = np.sqrt(n)*Istar
         scaled_unitary = U.dot(I)
 
-        # sigmas = np.zeros((n+1, n))
         sigmas = self.subtract(x, -scaled_unitary)
-
         return sigmas.T
 
 
@@ -442,8 +453,7 @@ def weights(self):
         """
 
         n = self.n
-
-        c = 1 / (n + 1)
+        c = 1. / (n + 1)
         W = np.full(n + 1, c)
 
         return W, W

filterpy/kalman/tests/test_ukf.py

@@ -32,7 +32,7 @@
 from math import cos, sin
 
 
-DO_PLOT = True
+DO_PLOT = False
 
 
 def test_sigma_plot():
@@ -840,13 +840,13 @@ def fx(x, dt):
 if __name__ == "__main__":
 
     DO_PLOT = True
-
+    #test_sigma_plot()
     # test_linear_1d()
     # test_batch_missing_data()
     #
     # test_linear_2d()
     # test_julier_sigma_points_1D()
-    # test_simplex_sigma_points_1D()
+    #test_simplex_sigma_points_1D()
     # test_fixed_lag()
     # DO_PLOT = True
     # test_rts()
@@ -866,7 +866,7 @@ def fx(x, dt):
 
     xi,w = sigma_points (x,P,kappa)
     xm, cov = unscented_transform(xi, w)'''
-    test_radar()
+    #test_radar()
     # test_sigma_plot()
     # test_julier_weights()
     # test_scaled_weights()
@@ -877,6 +877,6 @@ def fx(x, dt):
     print(xm)
     print(cov)"""
 #    sigma_points ([5,2],9*np.eye(2), 2)
-    plt.legend()
-    plt.show()
+    #plt.legend()
+    #plt.show()