From d56849d17bf0943deb5290c77e02c092494755f6 Mon Sep 17 00:00:00 2001 From: poeticcapybara Date: Mon, 1 Aug 2016 18:14:42 +0200 Subject: [PATCH 1/2] Simplex Points as alternative to Julier and Van der Merwe Points --- filterpy/kalman/UKF.py | 13 ++- filterpy/kalman/sigma_points.py | 136 +++++++++++++++++++++++++++++- filterpy/kalman/tests/test_ukf.py | 109 ++++++++++++++++++------ 3 files changed, 226 insertions(+), 32 deletions(-) diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py index b96ee4f..ccf71a5 100644 --- a/filterpy/kalman/UKF.py +++ b/filterpy/kalman/UKF.py @@ -179,6 +179,7 @@ def residual(a, b): y = 2*np.pi return y + References ---------- @@ -206,11 +207,14 @@ def residual(a, b): self.P = eye(dim_x) self._dim_x = dim_x self._dim_z = dim_z + self.points_fn = points self._dt = dt - self._num_sigmas = 2*dim_x + 1 + if self.points_fn.name == 'Simplex': + self._num_sigmas = dim_x + 1 + else: + self._num_sigmas = 2*dim_x + 1 self.hx = hx self.fx = fx - self.points_fn = points self.x_mean = x_mean_fn self.z_mean = z_mean_fn @@ -234,7 +238,8 @@ def residual(a, b): # sigma points transformed through f(x) and h(x) # variables for efficiency so we don't recreate every update - self.sigmas_f = zeros((2*self._dim_x+1, self._dim_x)) + + self.sigmas_f = zeros((self._num_sigmas, self._dim_x)) self.sigmas_h = zeros((self._num_sigmas, self._dim_z)) @@ -484,7 +489,7 @@ def rts_smoother(self, Xs, Ps, Qs=None, dt=None): # smoother gain Ks = zeros((n,dim_x,dim_x)) - num_sigmas = 2*dim_x + 1 + num_sigmas = self._num_sigmas xs, ps = Xs.copy(), Ps.copy() sigmas_f = zeros((num_sigmas, dim_x)) diff --git a/filterpy/kalman/sigma_points.py b/filterpy/kalman/sigma_points.py index 73778ce..1aa24bd 100644 --- a/filterpy/kalman/sigma_points.py +++ b/filterpy/kalman/sigma_points.py @@ -14,10 +14,10 @@ for more information. """ +from __future__ import division import numpy as np from scipy.linalg import cholesky - class MerweScaledSigmaPoints(object): def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None): @@ -76,6 +76,7 @@ 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 @@ -218,6 +219,7 @@ 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 @@ -313,3 +315,135 @@ def weights(self): W = np.full(2*n+1, .5 / (n + k)) W[0] = k / (n+k) return W, W + + +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 + + Parameters + ---------- + + n : int + Dimensionality of the state. n+1 weights will be generated. + + sqrt_method : function(ndarray), default=scipy.linalg.cholesky + Defines how we compute the square root of a matrix, which has + no unique answer. Cholesky is the default choice due to its + speed. Typically your alternative choice will be + scipy.linalg.sqrtm + + If your method returns a triangular matrix it must be upper + triangular. Do not use numpy.linalg.cholesky - for historical + reasons it returns a lower triangular matrix. The SciPy version + does the right thing. + + subtract : callable (x, y), optional + Function that computes the difference between x and y. + You will have to supply this if your state variable cannot support + subtraction, such as angles (359-1 degreees is 2, not 358). x and y + are state vectors, not scalars. + + References + ---------- + + .. [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 + else: + self.sqrt = sqrt_method + + if subtract is None: + self.subtract= np.subtract + else: + self.subtract = subtract + + + 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. + Returns tuple of the sigma points and weights. + + Works with both scalar and array inputs: + sigma_points (5, 9, 2) # mean 5, covariance 9 + sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I + + Parameters + ---------- + + X An array-like object of the means of length n + Can be a scalar if 1D. + examples: 1, [1,2], np.array([1,2]) + + P : scalar, or np.array + Covariance of the filter. If scalar, is treated as eye(n)*P. + + Returns + ------- + + sigmas : np.array, of size (n, n+1) + Two dimensional array of sigma points. Each column contains all of + the sigmas for one dimension in the problem space. + + Ordered by Xi_0, Xi_{1..n} + """ + + assert self.n == np.size(x), "expected size {}, but size is {}".format( + self.n, np.size(x)) + + n = self.n + + if np.isscalar(x): + x = np.asarray([x]) + x = x.reshape(-1, 1) + if np.isscalar(P): + P = np.eye(n)*P + else: + P = np.asarray(P) + + U = self.sqrt(P) + + 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)) + row[0, -1] = -d / np.sqrt(lambda_ * d * (d + 1)) + Istar = np.r_[np.c_[Istar, np.zeros((Istar.shape[0]))], row] + + 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 + + + def weights(self): + """ Computes the weights for the scaled unscented Kalman filter. + + Returns + ------- + + Wm : ndarray[n+1] + weights for mean + + Wc : ndarray[n+1] + weights for the covariances + """ + + n = self.n + + 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 6ca1621..21b4577 100644 --- a/filterpy/kalman/tests/test_ukf.py +++ b/filterpy/kalman/tests/test_ukf.py @@ -22,16 +22,17 @@ import matplotlib.pyplot as plt import numpy.random as random from numpy.random import randn +from numpy import asarray import numpy as np from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import (unscented_transform, MerweScaledSigmaPoints, - JulierSigmaPoints) + JulierSigmaPoints, SimplexSigmaPoints) from filterpy.common import Q_discrete_white_noise import filterpy.stats as stats from math import cos, sin -DO_PLOT = False +DO_PLOT = True def test_sigma_plot(): @@ -47,12 +48,18 @@ def test_sigma_plot(): sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) + sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3) + sp3 = SimplexSigmaPoints(n=2) w0, _ = sp0.weights() w1, _ = sp1.weights() + w2, _ = sp2.weights() + w3, _ = sp3.weights() - Xi0 = sp0.sigma_points (x, P) - Xi1 = sp1.sigma_points (x, P) + Xi0 = sp0.sigma_points(x, P) + Xi1 = sp1.sigma_points(x, P) + Xi2 = sp2.sigma_points(x, P) + Xi3 = sp3.sigma_points(x, P) assert max(Xi1[:,0]) > max(Xi0[:,0]) assert max(Xi1[:,1]) > max(Xi0[:,1]) @@ -62,16 +69,32 @@ def test_sigma_plot(): for i in range(Xi0.shape[0]): plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0], (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1], - color='blue') + color='blue', label='Julier low $\kappa$') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0], (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1], - color='green') + color='green', label='Julier high $\kappa$') + # for i in range(Xi2.shape[0]): + # plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0], + # (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1], + # color='red') + for i in range(Xi3.shape[0]): + plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0], + (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1], + color='black', label='Simplex') stats.plot_covariance_ellipse([1, 2], P) +def test_simplex_weights(): + for n in range(1,15): + for k in np.linspace(0,5,0.1): + Wm = UKF.weights(n, k) + + assert abs(sum(Wm) - 1) < 1.e-12 + + def test_julier_weights(): for n in range(1,15): for k in np.linspace(0,5,0.1): @@ -91,7 +114,7 @@ def test_scaled_weights(): assert abs(sum(Wc) - 1) < 1.e-1 -def test_sigma_points_1D(): +def test_julier_sigma_points_1D(): """ tests passing 1D data into sigma_points""" kappa = 0. @@ -106,8 +129,8 @@ def test_sigma_points_1D(): mean = 5 cov = 9 - Xi = sp.sigma_points (mean, cov) - xm, ucov = unscented_transform(Xi,Wm, Wc, 0) + Xi = sp.sigma_points(mean, cov) + xm, ucov = unscented_transform(Xi, Wm, Wc, 0) # sum of weights*sigma points should be the original mean m = 0.0 @@ -121,6 +144,34 @@ def test_sigma_points_1D(): assert Xi.shape == (3,1) +def test_simplex_sigma_points_1D(): + """ tests passing 1D data into sigma_points""" + + sp = SimplexSigmaPoints(1) + + #ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa) + + Wm, Wc = sp.weights() + assert np.allclose(Wm, Wc, 1e-12) + assert len(Wm) == 2 + + mean = 5 + cov = 9 + + Xi = sp.sigma_points(mean, cov) + xm, ucov = unscented_transform(Xi, Wm, Wc, 0) + + # sum of weights*sigma points should be the original mean + m = 0.0 + for x, w in zip(Xi, Wm): + m += x*w + + assert abs(m-mean) < 1.e-12 + assert abs(xm[0] - mean) < 1.e-12 + assert abs(ucov[0,0]-cov) < 1.e-12 + + assert Xi.shape == (2,1) + class RadarSim(object): def __init__(self, dt): @@ -145,11 +196,12 @@ def fx(x, dt): return A.dot(x) def hx(x): - return np.sqrt (x[0]**2 + x[2]**2) + return np.sqrt(x[0]**2 + x[2]**2) dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=0.) + # sp = SimplexSigmaPoints(n=3) kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 @@ -185,7 +237,6 @@ def hx(x): plt.subplot(312) plt.plot(t, xs[:,1]) plt.subplot(313) - plt.plot(t, xs[:,2]) @@ -206,7 +257,8 @@ def hx(x): dt = 0.1 - points = MerweScaledSigmaPoints(4, .1, 2., -1) + # points = MerweScaledSigmaPoints(4, .1, 2., -1) + points = SimplexSigmaPoints(n=4) kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) @@ -789,16 +841,17 @@ def fx(x, dt): DO_PLOT = True - test_linear_1d() - #test_batch_missing_data() - - #test_linear_2d() - #test_sigma_points_1D() - #test_fixed_lag() - #DO_PLOT = True - #test_rts() - #kf_circle() - #test_circle() + # test_linear_1d() + # test_batch_missing_data() + # + # test_linear_2d() + # test_julier_sigma_points_1D() + # test_simplex_sigma_points_1D() + # test_fixed_lag() + # DO_PLOT = True + # test_rts() + # kf_circle() + # test_circle() '''test_1D_sigma_points() @@ -813,15 +866,17 @@ def fx(x, dt): xi,w = sigma_points (x,P,kappa) xm, cov = unscented_transform(xi, w)''' - #test_radar() - #test_sigma_plot() - #test_julier_weights() - #test_scaled_weights() - + test_radar() + # test_sigma_plot() + # test_julier_weights() + # test_scaled_weights() + # test_simplex_weights() #print('xi=\n',Xi) """ xm, cov = unscented_transform(Xi, W) print(xm) print(cov)""" # sigma_points ([5,2],9*np.eye(2), 2) + plt.legend() + plt.show() From 888957f56d3ccb3a660991f871a6d2dbd3d17eb6 Mon Sep 17 00:00:00 2001 From: Roger Labbe Date: Wed, 14 Sep 2016 17:29:39 -0700 Subject: [PATCH 2/2] 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 c48d245..41013e1 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 1aa24bd..5ee176c 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 21b4577..85a7c24 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()