def show_sigma_transform(with_text=False): fig = plt.figure() ax=fig.gca() x = np.array([0, 5]) P = np.array([[4, -2.2], [-2.2, 3]]) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=9) sigmas = MerweScaledSigmaPoints(2, alpha=.5, beta=2., kappa=0.) S = sigmas.sigma_points(x=x, P=P) plt.scatter(S[:,0], S[:,1], c='k', s=80) x = np.array([15, 5]) P = np.array([[3, 1.2],[1.2, 6]]) plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.3) ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6)) ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6)) ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6)) ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6)) ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6)) ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) if with_text: plt.text(2.5, 1.5, r"$\chi$", fontsize=32) plt.text(13, -1, r"$\mathcal{Y}$", fontsize=32) #plt.axis('equal') plt.show()
def show_sigma_transform(): fig = plt.figure() ax=fig.gca() x = np.array([0, 5]) P = np.array([[4, -2.2], [-2.2, 3]]) plot_covariance_ellipse(x, P, facecolor='b', variance=9, alpha=0.5) S = UKF.sigma_points(x=x, P=P, kappa=0) plt.scatter(S[:,0], S[:,1], c='k', s=80) x = np.array([15, 5]) P = np.array([[3, 1.2],[1.2, 6]]) plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.5) ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6)) ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6)) ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6)) ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6)) ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6)) ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) #plt.axis('equal') 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 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_transform(with_text=False): fig = plt.figure() ax = fig.gca() x = np.array([0, 5]) P = np.array([[4, -2.2], [-2.2, 3]]) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=9) sigmas = MerweScaledSigmaPoints(2, alpha=.5, beta=2., kappa=0.) S = sigmas.sigma_points(x=x, P=P) plt.scatter(S[:, 0], S[:, 1], c='k', s=80) x = np.array([15, 5]) P = np.array([[3, 1.2], [1.2, 6]]) plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.3) ax.add_artist(arrow(S[0, 0], S[0, 1], 11, 4.1, 0.6)) ax.add_artist(arrow(S[1, 0], S[1, 1], 13, 7.7, 0.6)) ax.add_artist(arrow(S[2, 0], S[2, 1], 16.3, 0.93, 0.6)) ax.add_artist(arrow(S[3, 0], S[3, 1], 16.7, 10.8, 0.6)) ax.add_artist(arrow(S[4, 0], S[4, 1], 17.7, 5.6, 0.6)) ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) if with_text: plt.text(2.5, 1.5, r"$\chi$", fontsize=32) plt.text(13, -1, r"$\mathcal{Y}$", fontsize=32) #plt.axis('equal') plt.show()
def show_sigma_transform(): fig = plt.figure() ax = fig.gca() x = np.array([0, 5]) P = np.array([[4, -2.2], [-2.2, 3]]) plot_covariance_ellipse(x, P, facecolor='b', variance=9, alpha=0.5) S = UKF.sigma_points(x=x, P=P, kappa=0) plt.scatter(S[:, 0], S[:, 1], c='k', s=80) x = np.array([15, 5]) P = np.array([[3, 1.2], [1.2, 6]]) plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.5) ax.add_artist(arrow(S[0, 0], S[0, 1], 11, 4.1, 0.6)) ax.add_artist(arrow(S[1, 0], S[1, 1], 13, 7.7, 0.6)) ax.add_artist(arrow(S[2, 0], S[2, 1], 16.3, 0.93, 0.6)) ax.add_artist(arrow(S[3, 0], S[3, 1], 16.7, 10.8, 0.6)) ax.add_artist(arrow(S[4, 0], S[4, 1], 17.7, 5.6, 0.6)) ax.axes.get_xaxis().set_visible(False) ax.axes.get_yaxis().set_visible(False) #plt.axis('equal') plt.show()
def show_x_with_unobserved(): """ shows x=1,2,3 with velocity superimposed on top """ # plot velocity sigma=[0.5,1.,1.5,2] cov = np.array([[1,1],[1,1.1]]) stats.plot_covariance_ellipse ((2,2), cov=cov, variance=sigma, axis_equal=False) # plot positions cov = np.array([[0.003,0], [0,12]]) sigma=[0.5,1.,1.5,2] e = stats.covariance_ellipse (cov) stats.plot_covariance_ellipse ((1,1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse ((2,1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse ((3,1), ellipse=e, variance=sigma, axis_equal=False) # plot intersection cirle isct = Ellipse(xy=(2,2), width=.2, height=1.2, edgecolor='r', fc='None', lw=4) plt.gca().add_artist(isct) plt.ylim([0,11]) plt.xlim([0,4]) plt.xticks(np.arange(1,4,1)) plt.xlabel("Position") plt.ylabel("Time") plt.show()
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, .05, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .4, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50) plt.axis('equal') plt.xlim(0,10); plt.ylim(0,10) plt.show()
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, .05, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:, 0], sigmas[:, 1], c='k', s=50) x = np.array([5, 5]) points = MerweScaledSigmaPoints(2, .15, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:, 0], sigmas[:, 1], c='k', s=50) x = np.array([8, 5]) points = MerweScaledSigmaPoints(2, .4, 2., 1.) sigmas = points.sigma_points(x, P) plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5]) plt.scatter(sigmas[:, 0], sigmas[:, 1], c='k', s=50) plt.axis('equal') plt.xlim(0, 10) plt.ylim(0, 10) plt.show()
def show_x_error_chart(): """ displays x=123 with covariances showing error""" cov = np.array([[0.003, 0], [0, 12]]) sigma = [0.5, 1., 1.5, 2] e = stats.covariance_ellipse(cov) stats.plot_covariance_ellipse((1, 1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse((2, 1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse((3, 1), ellipse=e, variance=sigma, axis_equal=False) plt.ylim([0, 11]) plt.xticks(np.arange(1, 4, 1)) plt.xlabel("Position") plt.ylabel("Time") plt.show()
def show_x_with_unobserved(): """ shows x=1,2,3 with velocity superimposed on top """ # plot velocity sigma = [0.5, 1., 1.5, 2] cov = np.array([[1, 1], [1, 1.1]]) stats.plot_covariance_ellipse((2, 2), cov=cov, variance=sigma, axis_equal=False) # plot positions cov = np.array([[0.003, 0], [0, 12]]) sigma = [0.5, 1., 1.5, 2] e = stats.covariance_ellipse(cov) stats.plot_covariance_ellipse((1, 1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse((2, 1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse((3, 1), ellipse=e, variance=sigma, axis_equal=False) # plot intersection cirle isct = Ellipse(xy=(2, 2), width=.2, height=1.2, edgecolor='r', fc='None', lw=4) plt.gca().add_artist(isct) plt.ylim([0, 11]) plt.xlim([0, 4]) plt.xticks(np.arange(1, 4, 1)) plt.xlabel("Position") plt.ylabel("Time") plt.show()
def show_x_error_chart(): """ displays x=123 with covariances showing error""" cov = np.array([[0.003,0], [0,12]]) sigma=[0.5,1.,1.5,2] e = stats.covariance_ellipse (cov) stats.plot_covariance_ellipse ((1,1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse ((2,1), ellipse=e, variance=sigma, axis_equal=False) stats.plot_covariance_ellipse ((3,1), ellipse=e, variance=sigma, axis_equal=False) plt.ylim([0,11]) plt.xticks(np.arange(1,4,1)) plt.xlabel("Position") plt.ylabel("Time") plt.show()
def show_x_error_chart(count): """ displays x=123 with covariances showing error""" plt.cla() plt.gca().autoscale(tight=True) cov = np.array([[0.03,0], [0,8]]) e = stats.covariance_ellipse (cov) cov2 = np.array([[0.03,0], [0,4]]) e2 = stats.covariance_ellipse (cov2) cov3 = np.array([[12,11.95], [11.95,12]]) e3 = stats.covariance_ellipse (cov3) sigma=[1, 4, 9] if count >= 1: stats.plot_covariance_ellipse ((0,0), ellipse=e, variance=sigma) if count == 2 or count == 3: stats.plot_covariance_ellipse ((5,5), ellipse=e, variance=sigma) if count == 3: stats.plot_covariance_ellipse ((5,5), ellipse=e3, variance=sigma, edgecolor='r') if count == 4: M1 = np.array([[5, 5]]).T m4, cov4 = stats.multivariate_multiply(M1, cov2, M1, cov3) e4 = stats.covariance_ellipse (cov4) stats.plot_covariance_ellipse ((5,5), ellipse=e, variance=sigma, alpha=0.25) stats.plot_covariance_ellipse ((5,5), ellipse=e3, variance=sigma, edgecolor='r', alpha=0.25) stats.plot_covariance_ellipse (m4[:,0], ellipse=e4, variance=sigma) #plt.ylim([0,11]) #plt.xticks(np.arange(1,4,1)) plt.xlabel("Position") plt.ylabel("Velocity") plt.show()
def show_x_error_chart(count): """ displays x=123 with covariances showing error""" plt.cla() plt.gca().autoscale(tight=True) cov = np.array([[0.03, 0], [0, 8]]) e = stats.covariance_ellipse(cov) cov2 = np.array([[0.03, 0], [0, 4]]) e2 = stats.covariance_ellipse(cov2) cov3 = np.array([[12, 11.95], [11.95, 12]]) e3 = stats.covariance_ellipse(cov3) sigma = [1, 4, 9] if count >= 1: stats.plot_covariance_ellipse((0, 0), ellipse=e, variance=sigma) if count == 2 or count == 3: stats.plot_covariance_ellipse((5, 5), ellipse=e, variance=sigma) if count == 3: stats.plot_covariance_ellipse((5, 5), ellipse=e3, variance=sigma, edgecolor='r') if count == 4: M1 = np.array([[5, 5]]).T m4, cov4 = stats.multivariate_multiply(M1, cov2, M1, cov3) e4 = stats.covariance_ellipse(cov4) stats.plot_covariance_ellipse((5, 5), ellipse=e, variance=sigma, alpha=0.25) stats.plot_covariance_ellipse((5, 5), ellipse=e3, variance=sigma, edgecolor='r', alpha=0.25) stats.plot_covariance_ellipse(m4[:, 0], ellipse=e4, variance=sigma) #plt.ylim([0,11]) #plt.xticks(np.arange(1,4,1)) plt.xlabel("Position") plt.ylabel("Velocity") plt.show()