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 show_sigma_transform(with_text=False):
fig = plt.gcf()
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(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 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.Wm, sigmas.Wc
sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.)
S1 = sigmas.sigma_points(x, P)
Wm1, Wc1 = sigmas.Wm, sigmas.Wc
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()
def run_localization(
cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
sigma_bearing, ellipse_step=1, step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2,
sigma_bearing**2]*len(landmarks))
ukf.Q = np.eye(3)*0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1],
marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, u, dt/step, wheelbase)
track.append(sim_pos)
if i % step == 0:
ukf.predict(fx_args=u)
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='k', alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = sqrt(dx**2 + dy**2) + randn()*sigma_range
bearing = atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
randn()*sigma_bearing))
z.extend([d, a])
ukf.update(z, hx_args=(landmarks,))
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='g', alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:,1], color='k', lw=2)
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
return ukf
def run_localization(
cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
sigma_bearing, ellipse_step=1, step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2,
sigma_bearing**2]*len(landmarks))
ukf.Q = np.eye(3)*0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1],
marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, u, dt/step, wheelbase)
track.append(sim_pos)
if i % step == 0:
ukf.predict(fx_args=u)
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='k', alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = sqrt(dx**2 + dy**2) + randn()*sigma_range
bearing = atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
randn()*sigma_bearing))
z.extend([d, a])
ukf.update(z, hx_args=(landmarks,))
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='g', alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:,1], color='k', lw=2)
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
return ukf
def plotEllipseNew(mu, sigma, s):
sigma = sigma[:2, :2]
mu = mu[:2]
muT = (mu[0, 0], mu[1, 0])
print('muT' + str(muT))
plot_covariance_ellipse(muT, cov=sigma, variance=1.0, std=s)
return 0
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 plot3():
P = np.array([[6, 2.5], [2.5, .6]])
circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7)
ax = plt.gca()
ax.add_artist(circle1)
plt.xlim(0,10)
plt.ylim(0,3)
plt.axhline(3, ls='--')
stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
def run_localization(landmarks,
sigma_vel=0.1,
sigma_steer=np.radians(1),
sigma_range=0.3,
sigma_bearing=0.1,
dt=1.0,
x_init=[2, 6, .3]):
ekf = RobotEKF(dt,
wheelbase=0.5,
sigma_vel=sigma_vel,
sigma_steer=sigma_steer)
ekf.x = array([x_init]).T
ekf.P = np.diag([.1, .1, .1])
ekf.R = np.diag([sigma_range**2, sigma_bearing**2])
sim_pos = ekf.x.copy() # simulated position
u = array([1.1, .01]) # steering command (vel, steering angle radians)
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
for i in range(200):
sim_pos = ekf.move(sim_pos, u, dt / 10.) # simulate robot
plt.plot(sim_pos[0], sim_pos[1], ',', color='g')
if i % 10 == 0:
ekf.predict(u=u)
plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]),
ekf.P[0:2, 0:2],
std=6,
facecolor='b',
alpha=0.08)
x, y = sim_pos[0, 0], sim_pos[1, 0]
for lmark in landmarks:
d = np.sqrt((lmark[0] - x)**2 + (lmark[1] - y)**2)
a = atan2(lmark[1] - y, lmark[0] - x) - sim_pos[2, 0]
z = np.array([[d + np.random.randn() * sigma_range],
[a + np.random.randn() * sigma_bearing]])
ekf.update(z,
HJacobian=H_of,
Hx=Hx,
residual=residual,
args=(lmark),
hx_args=(lmark))
plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]),
ekf.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.4)
plt.axis('equal')
plt.show()
return ekf
def run_localization(landmarks,
std_vel,
std_steer,
std_range,
std_bearing,
step=10,
ellipse_step=20,
ylim=None):
ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel, std_steer=std_steer)
ekf.x = array([[2, 6, .3]]).T # x, y, steer angle
ekf.P = np.diag([.1, .1, .1])
ekf.R = np.diag([std_range**2, std_bearing**2])
sim_pos = ekf.x.copy() # simulated position
# steering command (vel, steering angle radians)
u = array([1.1, .07])
plt.figure()
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
track = []
for i in range(200):
sim_pos = ekf.move(sim_pos, u, dt / 10.) # simulate robot
track.append(sim_pos)
if i % step == 0:
ekf.predict(u=u)
if i % ellipse_step == 0:
plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]),
ekf.P[0:2, 0:2],
std=6,
facecolor='k',
alpha=0.3)
x, y = sim_pos[0, 0], sim_pos[1, 0]
for lmark in landmarks:
z = z_landmark(lmark, sim_pos, std_range, std_bearing)
ekf_update(ekf, z, lmark)
if i % ellipse_step == 0:
plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]),
ekf.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:, 1], color='k', lw=2)
plt.axis('equal')
plt.title("EKF Robot localization")
if ylim is not None: plt.ylim(*ylim)
plt.show()
return ekf
def plot_correlation_covariance():
P = [[4, 3.9], [3.9, 4]]
plot_covariance_ellipse((5, 10), P, edgecolor='k',
variance=[1, 2**2, 3**2])
plt.xlabel('X')
plt.ylabel('Y')
plt.gca().autoscale(tight=True)
plt.axvline(7.5, ls='--', lw=1)
plt.axhline(12.5, ls='--', lw=1)
plt.scatter(7.5, 12.5, s=1500, alpha=0.5)
plt.title('|4.0 3.9|\n|3.9 4.0|')
plt.show()
def plot_correlation_covariance():
P = [[4, 3.9], [3.9, 4]]
plot_covariance_ellipse((5, 10), P, edgecolor='k',
variance=[1, 2**2, 3**2])
plt.xlabel('X')
plt.ylabel('Y')
plt.gca().autoscale(tight=True)
plt.axvline(7.5, ls='--', lw=1)
plt.axhline(12.5, ls='--', lw=1)
plt.scatter(7.5, 12.5, s=1500, alpha=0.5)
plt.title('|4.0 3.9|\n|3.9 4.0|')
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 plot3():
P = np.array([[6, 2.5], [2.5, .6]])
circle1 = plt.Circle((10, 0),
3,
color='#004080',
fill=False,
linewidth=4,
alpha=.7)
ax = plt.gca()
ax.add_artist(circle1)
plt.xlim(0, 10)
plt.ylim(0, 3)
plt.axhline(3, ls='--')
stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
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 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 run_localization(cmds, x, x_guess, P, std_an, std_r, std_b,\
dt=1.0, ellipse_step=1, step=10, show=True):
if show:
plt.figure(figsize=(9, 6))
ukf = build_ukf(x_guess, P, std_r, std_b, dt=1.0)
sim_pos = x
# ideal_pos = ukf.x.copy()
np.random.seed(1)
# ideal = []
traj = []
xs = []
for i, u in enumerate(cmds):
# ideal_pos = move(ideal_pos, dt/step, u)
sim_pos = noisy_move(sim_pos, dt/step, u, std_an)
traj.append(sim_pos)
# ideal.append(ideal_pos)
if i % step == 0:
ukf.predict(u=u)
if i % ellipse_step == 0 and show:
cov = np.array([[ukf.P[0, 0], ukf.P[2, 0]],
[ukf.P[0, 2], ukf.P[2, 2]]])
plot_covariance_ellipse((ukf.x[0], ukf.x[2]), cov, std=6, facecolor='k', alpha=0.3)
z = noisy_sensor(sim_pos, std_r, std_b)
ukf.update(z)
if i % ellipse_step == 0 and show:
cov = np.array([[ukf.P[0, 0], ukf.P[2, 0]],
[ukf.P[0, 2], ukf.P[2, 2]]])
plot_covariance_ellipse((ukf.x[0], ukf.x[2]), cov, std=6, facecolor='g', alpha=0.8)
xs.append(ukf.x.copy())
xs = np.array(xs)
traj = np.array(traj)
# ideal = np.array(ideal)
if show:
plt.plot(traj[:, 0], traj[:, 2], color='b', linewidth=2, label='Actual trajectory')
plt.plot(xs[:, 0], xs[:, 2], 'r--', linewidth=2, label='Estimated trajectory')
# plt.plot(ideal[:, 0], ideal[:, 1], 'y', linewidth=2, label='Ideal trajectory')
plt.legend()
plt.title("UKF Robot Localization")
plt.axis([0, 750, 0, 500])
plt.xlabel('x (mm)')
plt.ylabel('y (mm)')
plt.grid()
plt.show()
return xs, traj
def plot_track_ellipses(N, zs, ps, cov, title):
#bp.plot_measurements(range(1,N + 1), zs)
#plt.plot(range(1, N + 1), ps, c='b', lw=2, label='filter')
plt.title(title)
for i,p in enumerate(cov):
plot_covariance_ellipse(
(i+1, ps[i]), cov=p, variance=4,
axis_equal=False, ec='g', alpha=0.5)
if i == len(cov)-1:
s = ('$\sigma^2_{pos} = %.2f$' % p[0,0])
plt.text (20, 5, s, fontsize=18)
s = ('$\sigma^2_{vel} = %.2f$' % p[1, 1])
plt.text (20, 0, s, fontsize=18)
plt.ylim(-5, 20)
plt.gca().set_aspect('equal')
def plot_3_covariances():
P = [[2, 0], [0, 2]]
plt.subplot(131)
plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2,
title='|2 0|\n|0 2|', axis_equal=False)
plt.ylim((4, 10))
plt.gca().set_aspect('equal', adjustable='box')
plt.subplot(132)
P = [[2, 0], [0, 9]]
plt.ylim((4, 10))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
axis_equal=False, title='|2 0|\n|0 9|')
plt.subplot(133)
P = [[2, 1.2], [1.2, 2]]
plt.ylim((4, 10))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
axis_equal=False,
title='|2 1.2|\n|1.2 2|')
plt.tight_layout()
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, .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_3_covariances():
P = [[2, 0], [0, 2]]
plt.subplot(131)
plt.gca().grid(b=False)
plt.gca().set_xticks([0,1,2,3,4])
plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2,
title='|2 0|\n|0 2|', std=[1,2,3], axis_equal=False)
plt.ylim((0, 15))
plt.gca().set_aspect('equal', adjustable='box')
plt.subplot(132)
plt.gca().grid(b=False)
plt.gca().set_xticks([0,1,2,3,4])
P = [[2, 0], [0, 6]]
plt.ylim((0, 15))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
std=[1,2,3],axis_equal=False, title='|2 0|\n|0 6|')
plt.subplot(133)
plt.gca().grid(b=False)
plt.gca().set_xticks([0,1,2,3,4])
P = [[2, 1.2], [1.2, 2]]
plt.ylim((0, 15))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
axis_equal=False,std=[1,2,3],
title='|2 1.2|\n|1.2 2|')
plt.tight_layout()
plt.show()
def plot_3_covariances():
P = [[2, 0], [0, 2]]
plt.subplot(131)
plt.gca().grid(b=False)
plt.gca().set_xticks([0,1,2,3,4])
plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2,
title='|2 0|\n|0 2|', std=[3], axis_equal=False)
plt.ylim((0, 15))
plt.gca().set_aspect('equal', adjustable='box')
plt.subplot(132)
plt.gca().grid(b=False)
plt.gca().set_xticks([0,1,2,3,4])
P = [[2, 0], [0, 6]]
plt.ylim((0, 15))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
std=[3],axis_equal=False, title='|2 0|\n|0 6|')
plt.subplot(133)
plt.gca().grid(b=False)
plt.gca().set_xticks([0,1,2,3,4])
P = [[2, 1.2], [1.2, 2]]
plt.ylim((0, 15))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
axis_equal=False,std=[3],
title='|2.0 1.2|\n|1.2 2.0|')
plt.tight_layout()
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, .09, 2., 1.)
sigmas = points.sigma_points(x, P)
Wm, Wc = points.Wm, points.Wc
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.Wm, points.Wc
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.Wm, points.Wc
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_interactive_covariance_multiply(x1, x2, x12, z1, z2, z12):
plt.figure(figsize=(9, 6))
P1 = [[x1, x12], [x12, x2]]
P2 = [[z1, z12], [z12, z2]]
P3 = multivariate_multiply((0, 0), P1, (0, 0), P2)[1]
plot_covariance_ellipse((0, 0), P1, ec='k', fc='y', alpha=0.6)
plot_covariance_ellipse((0, 0), P2, ec='k', fc='g', alpha=0.6)
plot_covariance_ellipse((0, 0), P3, ec='k', fc='b')
def plotResults(measurements, predictions, covariances):
for pos_vel, cov_matrix in zip(predictions, covariances):
cov = np.array([[cov_matrix[0, 0], cov_matrix[2, 0]],
[cov_matrix[0, 2], cov_matrix[2, 2]]])
# cov = np.array([[cov_matrix[1, 1], cov_matrix[3, 1]],[cov_matrix[1, 3], cov_matrix[3, 3]]])
[x_pos, x_vel, y_pos, y_vel] = pos_vel[:, 0]
mean = (x_pos, y_pos)
# Plotting elipses
plot_covariance_ellipse(mean,
cov=cov,
fc='r',
alpha=0.20,
show_semiaxis=True)
# plotting velocity
plt.quiver(x_pos,
y_pos,
x_vel,
y_vel,
color='b',
scale_units='xy',
scale=1,
alpha=0.50,
width=0.005)
# Plotting meditions
x = []
y = []
for measure in measurements:
if not measure: continue
x.append(measure[0])
y.append(measure[1])
plt.plot(x, y, '.g', lw=1, ls='--')
plt.axis('equal')
plt.show()
def example():
dt = 0.1
x = np.array([10.0, 4.5])
P = np.diag([500, 49])
F = np.array([[1, dt], [0, 1]])
# Q is the process noise
fig = plt.figure()
for _ in range(40):
# if the history should be deleted uncomment
#fig.clear()
plot_covariance_ellipse(x, P, edgecolor='r')
x, P = predict(x=x, P=P, F=F, Q=0)
print('x =', x)
plot_covariance_ellipse(x, P, edgecolor='k', ls='dashed')
fig.canvas.draw()
plt.show(block=False)
time.sleep(0.1)
plt.show()
def example():
dt = 0.1
x = np.array([10.0, 4.5])
P = np.diag([500, 49])
F = np.array([[1, dt], [0, 1]])
# Q is the process noise
fig = plt.figure()
for _ in range(40):
# if the history should be deleted uncomment
#fig.clear()
plot_covariance_ellipse(x, P, edgecolor='r')
x, P = predict(x=x, P=P, F=F, Q=0)
print('x =', x)
plot_covariance_ellipse(x, P, edgecolor='k', ls='dashed')
fig.canvas.draw()
plt.show(block=False)
time.sleep(0.1)
plt.show()
def run_localisation(ukf, landmarks):
global epoch
plt.figure()
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
track = []
while not stop:
print("epoch:", epoch)
#print("cov", ukf.P)
print("pos:", ukf.x)
epoch += 1
ukf.predict()
track.append((time.time(), ukf.x))
redis_send_pose(ukf.x)
if epoch % 5 == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.8)
time.sleep(0.5)
track = np.array(track)
#plt.plot(track[:, 0], track[:, 1], color='k', lw=2)
#plt.axis('equal')
#plt.title("UKF Robot localization")
#plt.show()
with open("track.txt", "w") as f:
f.write("timestamp, x, y, theta\n")
for t, p in track:
f.write("{}, {}, {}, {}\n".format(t, *p))
print(track)
return ukf
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 run(r):
landmarks = np.array([[5, 5], [5, 0], [10, 5]])
landmark_cov = np.diag([0.1, 0.1])
control_ideal = np.array([1, 1.1])
#plot landmark covariances
for l in landmarks:
plot_covariance_ellipse((l[0], l[1]),
landmark_cov,
std=6,
facecolor='k',
alpha=0.3)
track = []
for i in range(200):
print i
control = control_ideal # * [np.random.normal(1, 0.1),np.random.normal(1, 0.1)]
r.predict(control, control_ideal)
# print "yo"
# print track
track.append(r.x_act[:, 0])
if (i % 10 == 0):
plot_covariance_ellipse((r.x[0, 0], r.x[1, 0]),
r.P[0:2, 0:2],
std=6,
facecolor='k',
alpha=0.3)
for l in landmarks:
dist = np.sqrt((l[0] - r.x_act[0][0])**2 +
(l[1] - r.x_act[1][0])**2) + np.random.normal(
0, 0.1)
theta = np.arctan2(
l[1] - r.x_act[1][0], l[0] -
r.x_act[0][0]) - r.x_act[2][0] + np.random.normal(0, 0.02)
measurement = np.array([[dist], [theta]])
r.update(measurement, l, landmark_cov)
plot_covariance_ellipse((r.x[0][0], r.x[1][0]),
r.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.8)
#change direction
#control_ideal = np.array([np.random.normal(1, 0.5),np.random.normal(1, 0.5)])
#output the final covariance
print r.P
track = np.array(track)
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
plt.plot(track[:, 0], track[:, 1], color='k', lw=2)
plt.ylim((-10, 15))
plt.show()
def plot_3_covariances():
P = [[2, 0], [0, 2]]
plt.subplot(131)
plot_covariance_ellipse((2, 7),
cov=P,
facecolor='g',
alpha=0.2,
title='|2 0|\n|0 2|',
axis_equal=False)
plt.ylim((4, 10))
plt.gca().set_aspect('equal', adjustable='box')
plt.subplot(132)
P = [[2, 0], [0, 9]]
plt.ylim((4, 10))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7),
P,
facecolor='g',
alpha=0.2,
axis_equal=False,
title='|2 0|\n|0 9|')
plt.subplot(133)
P = [[2, 1.2], [1.2, 2]]
plt.ylim((4, 10))
plt.gca().set_aspect('equal', adjustable='box')
plot_covariance_ellipse((2, 7),
P,
facecolor='g',
alpha=0.2,
axis_equal=False,
title='|2 1.2|\n|1.2 2|')
plt.tight_layout()
plt.show()
if __name__=='__main__':
if False:
# fixme: vary R and Q
N = 30
sensor = PosSensor1 ((0, 0), (2, 1), 1.)
zs = np.array([np.array([sensor.read()]).T for _ in range(N)])
# run filter
robot_tracker = tracker1()
mu, cov, _, _ = robot_tracker.batch_filter(zs)
for x, P in zip(mu, cov):
# covariance of x and y
cov = np.array([[P[0, 0], P[2, 0]],
[P[0, 2], P[2, 2]]])
mean = (x[0, 0], x[2, 0])
plot_covariance_ellipse(mean, cov=cov, fc='g', alpha=0.15)
print robot_tracker.P
# plot results
zs *= .3048 # convert to meters
bp.plot_filter(mu[:, 0], mu[:, 2])
bp.plot_measurements(zs[:, 0], zs[:, 1])
plt.legend(loc=2)
plt.gca().set_aspect('equal')
plt.xlim((0, 20));
if True:
a()
kalman_filter.predict(u, wmega, dt)
kalman_filter.update(u, wmega, dt, tempZ)
output = kalman_filter.x
results.append(output)
output_listx.append(np.asscalar(output[0]))
output_listy.append(np.asscalar(output[1]))
line1.set_ydata(output_listy)
line1.set_xdata(output_listx)
lambda_, v = np.linalg.eig(kalman_filter.P[0:2, 0:2])
lambda_ = np.sqrt(lambda_)
plot_covariance_ellipse(np.array([(output[5]), (output[6])]),
kalman_filter.P[5:7, 5:7],
facecolor='lightgreen',
alpha=0.1)
plot_covariance_ellipse(np.array([(output[3]), (output[4])]),
kalman_filter.P[3:5, 3:5],
facecolor='lightcoral',
alpha=0.1)
plot_covariance_ellipse(np.array([(output[0]), (output[1])]),
kalman_filter.P[0:2, 0:2],
facecolor='lightskyblue',
alpha=0.1)
ax.scatter(np.asscalar(output[3]),
np.asscalar(output[4]),
color='r',
zorder=t)
ax.scatter(np.asscalar(output[5]),
# lists for the motion model Xk,Y
ListXk=[]
ListY=[]
X_pures=[]
Xk=X
Xpepe=X
ListXk.append(X)
X_pures.append(X)
fig = plt.figure()
ax10 = fig.add_subplot(111)
plot_covariance_ellipse([0,0],P, facecolor='lightskyblue', alpha=0.2)
# applying kalman filter
for t in range (0,N_iter):
w1=np.random.normal(0, np.sqrt(0), 1)
w2=np.random.normal(0, np.sqrt(0), 1)
# process noise
W =np.array([[w1[0]],[w2[0]]])
e=np.random.normal(0, np.sqrt(R), 1)
# motion model
Xk=np.matmul(F,Xk)+np.matmul(B,U)+np.matmul(G,W)
y0 = data["data_set"][2]["world"][2]
tracker.x = np.array([[x0, y0, 0, 0]]).T
tracker.P = np.eye(4) * 50.
return tracker
###############################################
zs = np.array([[d["world"][0], d["world"][2]] for d in data["data_set"]])
# run filter
robot_tracker = Tracker()
mu, cov, _, _ = robot_tracker.batch_filter(zs)
for x, P in zip(mu, cov):
# covariance of x and y
cov = np.array([[P[0, 0], P[2, 0]], [P[0, 2], P[2, 2]]])
mean = (x[0, 0], x[2, 0])
#print(x[0, 0], x[1, 0], x[2, 0], x[3, 0])
#print(cov)
plot_covariance_ellipse(mean, cov=cov, fc='g', std=3, alpha=0.5)
#plt.pause(0.1)
#plot results
plot_filter(mu[:, 0], mu[:, 2])
plot_measurements(zs[:, 0], zs[:, 1])
plt.legend(loc=2)
plt.xlim(0, 20)
plt.show()
# sigma[s_l:(s_l + 2), s_l:(s_l + 2)],
# std=6, facecolor='none', ec='#e74c3c', alpha=0.8)
# if (i == 2) and (len(sigma) >= 9):
# plot_covariance_ellipse(
# (u[s_l], u[(s_l + 1)]),
# sigma[s_l:(s_l + 2), s_l:(s_l + 2)],
# std=6, facecolor='none', ec='#f1c40f', alpha=0.8)
# if (i == 3) and (len(sigma) >= 11):
# plot_covariance_ellipse(
# (u[s_l], u[(s_l + 1)]),
# sigma[s_l:(s_l + 2), s_l:(s_l + 2)],
# std=6, facecolor='none', ec='#2c3e50', alpha=0.8)
# if (i == 4) and (len(sigma) >= 13):
# plot_covariance_ellipse(
# (u[s_l], u[(s_l + 1)]),
# sigma[s_l:(s_l + 2), s_l:(s_l + 2)],
# std=6, facecolor='none', ec='#2ecc71', alpha=0.8)
#Plot the robot covariance
plot_covariance_ellipse((float(u[0]), float(u[1])),
(sigma[0:2, 0:2]),
std=6,
facecolor='none',
ec='#004080',
alpha=0.3)
print(u)
print(sigma)
plt.axis([-1.0, 2.0, -1.0, 2.0])
plt.pause(0.01)
def run_localization(std_vel, std_steer, std_range, std_bearing, ylim=None):
ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel, std_steer=std_steer)
robot_x = 0.0
robot_y = 0.0
robot_theta = 0.0
ekf.x = np.array([[robot_x, robot_y, robot_theta]]).T # x, y, theta
ekf.P = np.diag([.1, .1, .1])
#NOISE
ekf.R = np.diag([1, 1])
M = np.diag([0.001, 0.1])
data_file = open("data.txt", 'r')
sensor_file = open("sensor.txt", 'r')
xr_file = open("xr.txt", 'r')
with data_file as f:
data = [tuple(map(float, i.split(','))) for i in f]
with sensor_file as f:
sensor = [tuple(map(float, i.split(','))) for i in f]
with xr_file as f:
xr = [tuple(map(float, i.split(','))) for i in f]
plt.figure()
initialize_map = bm.base_map()
initialize_map.draw_base_map()
steps = 0
for i in range(49):
delta_d, delta_theta = data[steps]
robot_vel = delta_d / dt
XJacobian = np.matrix([[1, 0, (-delta_d * math.sin(delta_theta))],
[0, 1, (delta_d * math.cos(delta_theta))],
[0, 0, 1]])
UJacobian = np.matrix([[(math.cos(delta_theta)), 0],
[(math.sin(delta_theta)), 0], [0, 1]])
# # steering command (velocity, heading)
u = np.array([robot_vel, delta_theta])
ekf.predict(XJacobian, UJacobian, M, u=u)
for j in range(5):
sensor_read = sense(steps, sensor)
rng = sensor_read[j][0]
bearing = sensor_read[j][1]
landmark_pos = initialize_map.get_landmark_pos(j)
lmark = [landmark_pos[1], landmark_pos[2]]
z_new = np.array([[rng], [bearing]])
# sim_pos = ekf.x.copy() # simulated position
# z = z_landmark(lmark, sim_pos)
#print z_new
ekf_update(ekf, z_new, lmark, rng)
plot_covariance_ellipse((ekf.x[0, 0], ekf.x[1, 0]),
ekf.P[0:2, 0:2],
std=6,
facecolor='none',
alpha=0.8)
steps += 1
#x_true is our real robot data
x_true = ask_the_oracle(steps, xr)
axis_marker = draw_axis_marker(math.degrees(x_true[2]))
axis_marker_predict = draw_axis_marker(math.degrees(ekf.x[2, 0]))
#plot the data
plt.scatter(x_true[0],
x_true[1],
marker=(3, 0, math.degrees(x_true[2]) + 26),
color='b',
s=200)
plt.scatter(x_true[0], x_true[1], marker=axis_marker, color='r', s=300)
plt.scatter(ekf.x[0, 0],
ekf.x[1, 0],
marker=(3, 0, (math.degrees(ekf.x[2, 0] + 26))),
color='y',
s=200)
plt.scatter(ekf.x[0, 0],
ekf.x[1, 0],
marker=axis_marker_predict,
color='r',
s=300)
plt.pause(dt)
plt.show()
return ekf
xp = ekf.x.copy()
plt.figure()
plt.scatter(m[:, 0], m[:, 1])
for i in range(500):
xp = ekf.move(xp, u, dt/10.)
plt.plot(xp[0], xp[1], ',', color='g')
if i % 10 == 0:
ekf.predict(u=u)
print('x',ekf.x[0,0])
print('y',ekf.x[1,0])
plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3,
facecolor='b', alpha=0.08)
for lmark in m:
d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r
a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h
z = np.array([[d], [a]])
ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual,
args=(lmark), hx_args=(lmark))
plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3,
facecolor='g', alpha=0.4)
plt.axis('equal')
plt.title("EKF Robot localization")
def plot1():
P = np.array([[6, 2.5], [2.5, .6]])
stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
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((-9, 16))
#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((-9, 16))
#plt.ylim([0,11])
#plt.xticks(np.arange(1,4,1))
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.show()
track = [] for i, u in enumerate(cmds): sim_pos = move(sim_pos, dt/step, u, wheelbase) track.append(sim_pos) if i % step == 0: try: ukf.predict(u=u, wheelbase=wheelbase) except: # force positive definite matrix ukf.P = np.matmul(ukf.P, ukf.P.T) ukf.predict(u=u, wheelbase=wheelbase) if i % ellipse_step == 0: plot_covariance_ellipse( (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, facecolor='k', alpha=0.3) x, y = sim_pos[0], sim_pos[1] z = [] for lmark in landmarks: dx, dy = lmark[0] - x, lmark[1] - y d = np.sqrt(dx**2 + dy**2) + np.random.randn()*sigma_range bearing = np.arctan2(lmark[1] - y, lmark[0] - x) a = (normalize_angle(bearing - sim_pos[2] + np.random.randn()*sigma_bearing)) z.extend([d, a]) ukf.update(z, landmarks=landmarks) if i % ellipse_step == 0: plot_covariance_ellipse(
def plot1():
stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2)
def show_x_error_chart(count):
""" displays x=123 with covariances showing error"""
plt.cla()
plt.gca().autoscale(tight=True)
cov = np.array([[0.1, 0], [0, 8]])
pos_ellipse = stats.covariance_ellipse(cov)
cov2 = np.array([[0.1, 0], [0, 4]])
cov3 = np.array([[12, 11.95], [11.95, 12]])
vel_ellipse = stats.covariance_ellipse(cov3)
sigma = [1, 4, 9]
if count < 4:
stats.plot_covariance_ellipse((0, 0),
ellipse=pos_ellipse,
variance=sigma)
if count == 2:
stats.plot_covariance_ellipse((0, 0),
ellipse=vel_ellipse,
variance=sigma,
edgecolor='r')
if count == 3:
stats.plot_covariance_ellipse((5, 5),
ellipse=pos_ellipse,
variance=sigma)
stats.plot_covariance_ellipse((0, 0),
ellipse=vel_ellipse,
variance=sigma,
edgecolor='r')
if count == 4:
M0 = np.array([[0, 0]]).T
M1 = np.array([[5, 5]]).T
_, cov4 = stats.multivariate_multiply(M0, cov2, M1, cov3)
e4 = stats.covariance_ellipse(cov4)
stats.plot_covariance_ellipse((0, 0),
ellipse=pos_ellipse,
variance=sigma,
alpha=0.25)
stats.plot_covariance_ellipse((5, 5),
ellipse=pos_ellipse,
variance=sigma,
alpha=0.25)
stats.plot_covariance_ellipse((0, 0),
ellipse=vel_ellipse,
variance=sigma,
edgecolor='r',
alpha=0.25)
stats.plot_covariance_ellipse((5, 5), ellipse=e4, variance=sigma)
plt.ylim((-9, 16))
plt.xlabel("Position")
plt.ylabel("Velocity")
plt.axis('equal')
plt.show()
