From 888957f56d3ccb3a660991f871a6d2dbd3d17eb6 Mon Sep 17 00:00:00 2001 From: Roger Labbe Date: Wed, 14 Sep 2016 17:29:39 -0700 Subject: [PATCH] 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 | 5 +---- filterpy/kalman/sigma_points.py | 34 ++++++++++++++++++++----------- filterpy/kalman/tests/test_ukf.py | 12 +++++------ 3 files changed, 29 insertions(+), 22 deletions(-) diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py index c48d245d..41013e12 100644 --- a/filterpy/kalman/UKF.py +++ b/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 diff --git a/filterpy/kalman/sigma_points.py b/filterpy/kalman/sigma_points.py index 1aa24bd9..5ee176cb 100644 --- a/filterpy/kalman/sigma_points.py +++ b/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 diff --git a/filterpy/kalman/tests/test_ukf.py b/filterpy/kalman/tests/test_ukf.py index 21b4577a..85a7c24c 100644 --- a/filterpy/kalman/tests/test_ukf.py +++ b/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()