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()
