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