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