diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py index 28fffa8..41013e1 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,11 @@ 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 + self._num_sigmas = points.num_sigmas() self.hx = hx self.fx = fx - self.points_fn = points self.x_mean = x_mean_fn self.z_mean = z_mean_fn @@ -234,7 +235,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)) @@ -473,7 +475,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..5ee176c 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): @@ -90,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. @@ -230,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. @@ -313,3 +323,137 @@ 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.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 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. + 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 = 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..85a7c24 100644 --- a/filterpy/kalman/tests/test_ukf.py +++ b/filterpy/kalman/tests/test_ukf.py @@ -22,10 +22,11 @@ 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 @@ -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) @@ -788,17 +840,18 @@ def fx(x, dt): if __name__ == "__main__": 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_sigma_plot() + # 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() @@ -814,14 +867,16 @@ 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_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()