def plot_sigma_points(): x = np.array([0, 0]) P = np.array([[4, 2], [2, 4]]) sigmas = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=1.) S0 = sigmas.sigma_points(x, P) Wm0, Wc0 = sigmas.weights() sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.) S1 = sigmas.sigma_points(x, P) Wm1, Wc1 = sigmas.weights() def plot_sigmas(s, w, **kwargs): min_w = min(abs(w)) scale_factor = 100 / min_w return plt.scatter(s[:, 0], s[:, 1], s=abs(w)*scale_factor, alpha=.5, **kwargs) plt.subplot(121) plot_sigmas(S0, Wc0, c='b') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=0.3') plt.subplot(122) plot_sigmas(S1, Wc1, c='b', label='Kappa=2') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=1') plt.show() print(sum(Wc0))
def show_sigma_selections(): ax=plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .09, 2., 1.) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 1., .15) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .2, 3., 10) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') plt.axis('equal') plt.xlim(0,10); plt.ylim(0,10) plt.show()
def plot_sigma_points(): x = np.array([0, 0]) P = np.array([[4, 2], [2, 4]]) sigmas = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=1.) S0 = sigmas.sigma_points(x, P) Wm0, Wc0 = sigmas.weights() sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.) S1 = sigmas.sigma_points(x, P) Wm1, Wc1 = sigmas.weights() def plot_sigmas(s, w, **kwargs): min_w = min(abs(w)) scale_factor = 100 / min_w return plt.scatter(s[:, 0], s[:, 1], s=abs(w) * scale_factor, alpha=.5, **kwargs) plt.subplot(121) plot_sigmas(S0, Wc0, c='b') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=0.3') plt.subplot(122) plot_sigmas(S1, Wc1, c='b', label='Kappa=2') plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4]) plt.title('alpha=1') plt.show() print(sum(Wc0))
def show_sigma_selections(): ax = plt.gca() ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) x = np.array([2, 5]) P = np.array([[3, 1.1], [1.1, 4]]) points = MerweScaledSigmaPoints(2, .09, 2., 1.) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 1., .15) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .2, 3., 10) sigmas = points.sigma_points(x, P) Wm, Wc = points.weights() plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5]) _plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k') plt.axis('equal') plt.xlim(0, 10) plt.ylim(0, 10) plt.show()
def print_sigmas(n=1, mean=5, cov=3, alpha=.1, beta=2., kappa=2): points = MerweScaledSigmaPoints(n, alpha, beta, kappa) print('sigmas: ', points.sigma_points(mean, cov).T[0]) Wm, Wc = points.weights() print('mean weights:', Wm) print('cov weights:', Wc) print('lambda:', alpha**2 *(n+kappa) - n) print('sum cov', sum(Wc))
def print_sigmas(n=1, mean=5, cov=3, alpha=.1, beta=2., kappa=2): points = MerweScaledSigmaPoints(n, alpha, beta, kappa) print('sigmas: ', points.sigma_points(mean, cov).T[0]) Wm, Wc = points.weights() print('mean weights:', Wm) print('cov weights:', Wc) print('lambda:', alpha**2 * (n + kappa) - n) print('sum cov', sum(Wc))
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together 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) # test __repr__ doesn't crash str(sp0) str(sp1) str(sp2) str(sp3) w0, _ = sp0.weights() w1, _ = sp1.weights() w2, _ = sp2.weights() w3, _ = sp3.weights() 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]) if DO_PLOT: plt.figure() 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', 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', 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_scaled_weights(): for n in range(1, 5): for alpha in np.linspace(0.99, 1.01, 100): for beta in range(0, 2): for kappa in range(0, 2): sp = MerweScaledSigmaPoints(n, alpha, 0, 3 - n) Wm, Wc = sp.weights() assert abs(sum(Wm) - 1) < 1.e-1 assert abs(sum(Wc) - 1) < 1.e-1
def test_scaled_weights(): for n in range(1,5): for alpha in np.linspace(0.99, 1.01, 100): for beta in range(0,2): for kappa in range(0,2): sp = MerweScaledSigmaPoints(n, alpha, 0, 3-n) Wm, Wc = sp.weights() assert abs(sum(Wm) - 1) < 1.e-1 assert abs(sum(Wc) - 1) < 1.e-1
def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together 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) 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]) if DO_PLOT: plt.figure() 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', 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', 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 plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.): def fx(x): return x**3 def dfx(x): return 3*x**2 mean = 1 var = .1 std = math.sqrt(var) data = normal(loc=mean, scale=std, size=50000) d_t = fx(data) points = MerweScaledSigmaPoints(1, alpha, beta, kappa) Wm, Wc = points.weights() sigmas = points.sigma_points(mean, var) sigmas_f = np.zeros((3, 1)) for i in range(3): sigmas_f[i] = fx(sigmas[i, 0]) ### pass through unscented transform ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc) ukf_mean = ukf_mean[0] ukf_std = math.sqrt(ukf_cov[0]) norm = scipy.stats.norm(ukf_mean, ukf_std) xs = np.linspace(-3, 5, 200) plt.plot(xs, norm.pdf(xs), ls='--', lw=1, color='b') plt.hist(d_t, bins=200, normed=True, histtype='step', lw=1, color='g') actual_mean = d_t.mean() plt.axvline(actual_mean, lw=1, color='g', label='Monte Carlo') plt.axvline(ukf_mean, lw=1, ls='--', color='b', label='UKF') plt.legend() plt.show() print('actual mean={:.2f}, std={:.2f}'.format(d_t.mean(), d_t.std())) print('UKF mean={:.2f}, std={:.2f}'.format(ukf_mean, ukf_std))
mean = (0, 0) p = np.array([[32, 15], [15., 40.]]) # Compute linearized mean mean_fx = f_nonlinear_xy(*mean) #generate random points xs, ys = multivariate_normal(mean=mean, cov=p, size=10000).T #initial mean and covariance mean = (0, 0) p = np.array([[32., 15], [15., 40.]]) # create sigma points - we will learn about this later points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.) Wm, Wc = points.weights() sigmas = points.sigma_points(mean, p) ### pass through nonlinear function sigmas_f = np.empty((5, 2)) for i in range(5): sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1]) ### use unscented transform to get new mean and covariance ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc) #generate random points np.random.seed(100) xs, ys = multivariate_normal(mean=mean, cov=p, size=5000).T plt.figure()