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)
w0, _ = sp0.weights()
w1, _ = sp1.weights()
Xi0 = sp0.sigma_points(x, P)
Xi1 = sp1.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')
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')
stats.plot_covariance_ellipse([1, 2], P)
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 = UKF.weights(2, kappa)
sp1 = UKF.weights(2, kappa*1000)
Xi0 = UKF.sigma_points (x, P, kappa)
Xi1 = UKF.sigma_points (x, P, kappa*1000)
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])*sp0[i] + x[0, 0],
(Xi0[i,1]-x[0, 1])*sp0[i] + x[0, 1],
color='blue')
for i in range(Xi1.shape[0]):
plt.scatter((Xi1[i, 0]-x[0, 0]) * sp1[i] + x[0,0],
(Xi1[i, 1]-x[0, 1]) * sp1[i] + x[0,1],
color='green')
stats.plot_covariance_ellipse([1, 2], P)
def fx(x, dt):
return x
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.)
kf.x = np.array([100, 100.])
kf.P *= 40
hx.p = kf.x - np.array([50, 50])
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
stats.plot_covariance_ellipse(kf.x,
cov=kf.P,
axis_equal=True,
facecolor='y',
edgecolor=None,
alpha=0.6)
plt.scatter([100], [100], c='y', label='Initial')
kf.R[0, 0] = radians(1)**2
kf.R[1, 1] = 2.**2
kf.predict()
kf.update(np.array([radians(45), d]))
print(kf.x)
print(kf.P)
stats.plot_covariance_ellipse(kf.x,
cov=kf.P,
def fx(x,dt):
return x
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.)
kf.x = np.array([100, 100.])
kf.P *= 40
hx.p = kf.x - np.array([50,50])
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
stats.plot_covariance_ellipse(
kf.x, cov=kf.P, axis_equal=True,
facecolor='y', edgecolor=None, alpha=0.6)
plt.scatter([100], [100], c='y', label='Initial')
kf.R[0,0] = radians (1)**2
kf.R[1,1] = 2.**2
kf.predict()
kf.update(np.array([radians(45), d]))
print(kf.x)
print(kf.P)
stats.plot_covariance_ellipse(
kf.x, cov=kf.P, axis_equal=True,
