def draw_trajectory(self,
xs,
mus=None,
Sigmas=None,
color=array((1.0, 0.0, 0.0, 0.2))):
T = xs.shape[1]
XYZ = mat(zeros((3, T)))
for t in range(T):
XYZ[:, t] = self.traj_pos(xs[:, t])
if mus != None and Sigmas != None:
for t in range(T):
mu_y, Sigma_y = unscented_transform(mus[:,t], Sigmas[:,:,t],\
lambda x: self.traj_pos(x))
# padding for positive definiteness
Sigma_y = Sigma_y + 0.0001 * identity(3)
self.handles.append(draw_ellipsoid(mu_y, Sigma_y, std_dev=2,\
env=self.env, colors=color))
#self.handles.append(self.env.drawlinestrip(points=array(((xyz[0], xyz[1], xyz[2]),(0.0, 0.0,0.0))),
# linewidth=3.0))
self.handles.append(
self.env.drawlinestrip(points=XYZ.T,
linewidth=3.0,
colors=color[0:3]))
def draw_trajectory(self, xs, mus=None, Sigmas=None, color=array((1.0, 0.0, 0.0, 0.2))):
T = xs.shape[1]
XYZ = mat(zeros((3,T)))
for t in range(T):
XYZ[:,t] = self.traj_pos(xs[:,t])
if mus != None and Sigmas != None:
for t in range(T):
mu_y, Sigma_y = unscented_transform(mus[:,t], Sigmas[:,:,t],\
lambda x: self.traj_pos(x))
# padding for positive definiteness
Sigma_y = Sigma_y + 0.0001 * identity(3)
self.handles.append(draw_ellipsoid(mu_y, Sigma_y, std_dev=2,\
env=self.env, colors=color))
#self.handles.append(self.env.drawlinestrip(points=array(((xyz[0], xyz[1], xyz[2]),(0.0, 0.0,0.0))),
# linewidth=3.0))
self.handles.append(self.env.drawlinestrip(points=XYZ.T, linewidth=3.0, colors=color[0:3]))
hs = list() # Measurement function for each camera
zs = list() # Simulated measurements from each camera
for cam in cams:
hs.append(cam.project_point)
#TODO Better measurement simulation
zs.append(hs[-1](mus[0]) + (rand() - 0.5) * 10)
# Indexing doesn't follow convention, z_k measurement of mu_k
mu1, Sigma1 = ekf_update(f, hs[0], Q, R, mus[0], Sigmas[0], u, zs[0])
mus.append(mu1)
Sigmas.append(Sigma1)
for k in range(1, len(cams)):
print zs[k]
mu1, Sigma1 = ekf_update(f, hs[k], Q, R, mus[1], Sigmas[1], u, zs[k])
mus.append(mu1)
Sigmas.append(Sigma1)
# Display
world.display()
axes().set_xlim(-1, 1)
axes().set_ylim(-1, 1)
conf = 0.95
for k in xrange(len(mus)):
draw_ellipsoid(mus[k], Sigmas[k], conf, color=COLORS[k % len(COLORS)])
print COLORS[k % len(COLORS)]
show()
f = lambda x, u: x # Dynamics
hs = list() # Measurement function for each camera
zs = list() # Simulated measurements from each camera
for cam in cams:
hs.append(cam.project_point)
#TODO Better measurement simulation
zs.append(hs[-1](mus[0]) + (rand()-0.5)*10)
# Indexing doesn't follow convention, z_k measurement of mu_k
mu1, Sigma1 = ekf_update(f, hs[0], Q, R, mus[0], Sigmas[0], u, zs[0])
mus.append(mu1); Sigmas.append(Sigma1)
for k in range(1, len(cams)):
print zs[k]
mu1, Sigma1 = ekf_update(f, hs[k], Q, R, mus[1], Sigmas[1], u, zs[k])
mus.append(mu1); Sigmas.append(Sigma1)
# Display
world.display()
axes().set_xlim(-1, 1);
axes().set_ylim(-1, 1);
conf = 0.95
for k in xrange(len(mus)):
draw_ellipsoid(mus[k], Sigmas[k], conf, color=COLORS[k%len(COLORS)])
print COLORS[k%len(COLORS)]
show()
