Sigmas = np.zeros((Q.shape[0], Q.shape[1], T)) Sigmas[:, :, 0] = np.mat(np.diag([0.001] * num_states)) #arg # Generate trajectory and obtain observations along trajectory X_bar = np.mat(np.zeros((car.NX, T))) #arg X_bar[:, 0] = np.mat(x0).T U_bar = np.ones((car.NU, T - 1)) * 1.7 #arg for t in xrange(1, T): U_bar[1, t - 1] = 0 for t in xrange(1, T): X_bar[:,t] = np.mat(car.dynamics(X_bar[:,t-1], U_bar[:, t-1])) +\ np.mat(dynamics_noise[:,t-1]).T z = car.observe(s, x=X_bar[:, t]) + np.mat(measurement_noise[:, t - 1]).T mus[:, t], Sigmas[:, :, t] = ekf_update(car.dynamics, lambda x: car.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U_bar[:, t - 1], z) # Plot nominal trajectory with covariance ellipses ax = plt.gca() plt.title('Nominal') s.draw(ax=ax) #print Sigmas car.draw_trajectory(mat2tuple(X_bar.T), mus=mus, Sigmas=Sigmas[0:2, 0:2, :]) plt.show()
X_bar = np.mat(np.zeros((car.NX, T))) #arg X_bar[:,0] = np.mat(x0).T U_bar = np.load('data/nominal_trajectory.npy') #U_bar = np.random.random((car.NU, T-1)) #for t in xrange(1,T): #U_bar[1,t-1] = (U_bar[1,t-1] - 0.5) #np.save('data/nominal_trajectory', U_bar) ''' X_bar = np.mat(d['dat_states']) U_bar = np.mat(d['dat_ctrls']) for t in xrange(1,T): X_bar[:,t] = np.mat(car.dynamics(X_bar[:,t-1], U_bar[:, t-1])) +\ np.mat(dynamics_noise[:,t-1]).T mus[:,t], Sigmas[:,:,t] = ekf_update(car.dynamics, lambda x: car.observe(s, x=x), Q, R, mus[:,t-1], Sigmas[:,:,t-1], U_bar[:,t-1], None) #NOTE No obs # Plot nominal trajectory with covariance ellipses ax = plt.gca() s.draw(ax=ax) #print Sigmas #car.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:]) Bel_bar = np.mat(np.zeros((car.NB, T))) for t in xrange(T): Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t]))) As, Bs, Cs = car.linearize_belief_dynamics_trajectory(Bel_bar, U_bar, s, Q, R) for t in xrange(T-1): # Overwrites Bel_bar
# Sigmas[2,2,0] = 0.0000001 X = np.mat(np.zeros((arm.NX, T))) X[:, 0] = np.mat(x0).T U = np.mat(np.zeros((arm.NU, T - 1))) for t in range(T - 1): U[:, t] = du # U[:,T/2:] = -2 * U[:,T/2:] # U = np.mat(np.random.random_sample((arm.NU, T-1))/5) # U[:,10:] = -2 * U[:,10:] for t in xrange(1, T): X[:, t] = arm.dynamics(X[:, t - 1], U[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update( arm.dynamics, lambda x: arm.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U[:, t - 1], None ) # NOTE No obs arm.draw_trajectory(X, mus, Sigmas) Bel_bar = np.mat(np.zeros((arm.NB, T))) for t in xrange(T): Bel_bar[:, t] = np.vstack((X[:, t], cov2vec(Sigmas[:, :, t]))) rho_bel = 0.2 rho_u = 0.3 N_iter = 1 goal_bel = np.copy(Bel_bar[:, -1]) goal_bel[arm.NX :] = 0 opt_bels, opt_ctrls, opt_vals = scp_solver_beliefs(
Sigmas.append(diag([.05, .05])) # In m Q = diag([.0, .0]) # In m R = 100 # In pixels u = zeros2((2, 1)) # Static dynamics/no controls 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
X = np.mat(np.zeros((localizer.NX, T))) X[:, 0] = x0 U = np.mat(np.zeros((localizer.NU, T - 1))) for t in range(T - 1): U[:, t] = du #U[:,T/2:] = -2 * U[:,T/2:] #U = np.mat(np.random.random_sample((arm.NU, T-1))/5) #U[:,10:] = -2 * U[:,10:] for t in xrange(1, T): X[:, t] = localizer.dynamics(X[:, t - 1], U[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update(localizer.dynamics, lambda x: localizer.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U[:, t - 1], None) #NOTE No obs localizer.draw_trajectory(X, mus, Sigmas) Bel_bar = np.mat(np.zeros((localizer.NB, T))) for t in xrange(T): Bel_bar[:, t] = np.vstack((X[:, t], cov2vec(Sigmas[:, :, t]))) rho_bel = 0.1 rho_u = 0.1 N_iter = 5 goal_bel = np.copy(Bel_bar[:, -1]) goal_bel[localizer.NX:] = 0 opt_bels, opt_ctrls, opt_vals = scp_solver_beliefs(s, Bel_bar.copy(), U,\
X = np.mat(np.zeros((arm.NX, T))) X[:, 0] = np.mat(x0).T U = np.mat(np.zeros((arm.NU, T - 1))) for t in range(T - 1): U[:, t] = du #U[:,T/2:] = -2 * U[:,T/2:] #U = np.mat(np.random.random_sample((arm.NU, T-1))/5) #U[:,10:] = -2 * U[:,10:] for t in xrange(1, T): X[:, t] = arm.dynamics(X[:, t - 1], U[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update(arm.dynamics, lambda x: arm.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U[:, t - 1], None) #NOTE No obs arm.draw_trajectory(X, mus, Sigmas) drawRobot = X getch = _Getch._Getch() t = 0 while True: c = getch() print c if c == 'x': break elif ord(c) == 3 or ord(c) == 4: exit(0) elif c == 'n':
Sigmas.append(diag([.05, .05])) # In m Q = diag([.0, .0]) # In m R = 100 # In pixels u = zeros2((2,1)) # Static dynamics/no controls 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)):
Sigmas[2,2,0] = 0.0000001 # Generate nominal belief trajectory X_bar = np.mat(np.zeros((localizer.NX, T))) #arg X_bar[:,0] = np.mat(x0).T U_bar = np.ones((localizer.NU, T-1))*0.35 for t in xrange(1,T): U_bar[1,t-1] = -0.005 #print U_bar for t in xrange(1,T): X_bar[:,t] = np.mat(localizer.dynamics(X_bar[:,t-1], U_bar[:, t-1])) +\ np.mat(dynamics_noise[:,t-1]).T mus[:,t], Sigmas[:,:,t] = ekf_update(localizer.dynamics, lambda x: localizer.observe(s, x=x), Q, R, mus[:,t-1], Sigmas[:,:,t-1], U_bar[:,t-1], None) #NOTE No obs # Plot nominal trajectory with covariance ellipses ax = plt.gca() s.draw(ax=ax) localizer.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:], color='yellow') localizer.draw_trajectory([], mus=X_bar[4:6,0:1], Sigmas=Sigmas[4:6,4:6,0:1], color='yellow') localizer.draw_trajectory([], mus=X_bar[4:6,T-2:T-1], Sigmas=Sigmas[4:6,4:6,T-2:T-1], color='yellow') #for t in range(0,T): # localizer.mark_fov(X_bar[:,t], s, [-1, 1, -1, 1], color=colors[t % len(colors)]) #plt.show() #stop
Sigmas[:,:,0] = np.mat(np.diag([0.005]*num_states)) #arg #Sigmas[2,2,0] = 0.0000001 #Sigmas[3,3,0] = 0.0000001 # Generate a nomimal trajectory X_bar = np.mat(np.zeros((links.NX, T))) X_bar[:,0] = x0; U_bar = np.mat(np.zeros((links.NU, T-1))) #U_bar = 10*np.mat(np.random.random_sample((links.NU, T-1))) - 5 for t in xrange(1,T): U_bar[0, t-1] = 1*float(t)/T U_bar[1, t-1] = 1.2 X_bar[:,t] = links.dynamics(X_bar[:,t-1], U_bar[:, t-1]) mus[:,t], Sigmas[:,:,t] = ekf_update(links.dynamics, lambda x: links.observe(s, x=x), Q, R, mus[:,t-1], Sigmas[:,:,t-1], U_bar[:,t-1], None) # Plot nominal trajectory #ax = plt.subplot(121) ax = plt.gca() s.draw(ax=ax) links.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:], color='red') #plt.show() #stop Bel_bar = np.mat(np.zeros((links.NB, T))) for t in xrange(T): Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t]))) goal_bel = np.copy(Bel_bar[:,-1])
#Sigmas[2,2,0] = 0.0000001 #Sigmas[3,3,0] = 0.0000001 # Generate a nomimal trajectory X_bar = np.mat(np.zeros((links.NX, T))) X_bar[:, 0] = x0 U_bar = np.mat(np.zeros((links.NU, T - 1))) #U_bar = 10*np.mat(np.random.random_sample((links.NU, T-1))) - 5 for t in xrange(1, T): U_bar[0, t - 1] = 1 * float(t) / T U_bar[1, t - 1] = 1.2 X_bar[:, t] = links.dynamics(X_bar[:, t - 1], U_bar[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update(links.dynamics, lambda x: links.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U_bar[:, t - 1], None) # Plot nominal trajectory #ax = plt.subplot(121) ax = plt.gca() s.draw(ax=ax) links.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2, 0:2, :], color='red') #plt.show() #stop Bel_bar = np.mat(np.zeros((links.NB, T)))
R = 100 # In pixels u = zeros2((2,1)) # Static dynamics/no controls f = lambda x, u: x # Dynamics hs = list() # Measurement function for each camera for cam in cams: # Second camera will be moving aronud but h[1] should change with it hs.append(cam.project_point) def sim_noise(): #return (rand()-0.5)*10 return 0 # 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, hs[0](mus[0]) + sim_noise()) mus.append(mu1); Sigmas.append(Sigma1) # Discretization/search dxy = 0.1 dtheta = .314 min_trace = inf z1 = hs[1](mus[0]) + sim_noise() for x in arange(-1, dxy, 1): for y in arange(-1, dxy, 1): if x == 0 and y == 0: # Object at focal pt. exception continue for theta in arange(-pi, pi, dtheta): if theta == 0: # Results in inf/error continue print x, y, theta
f = lambda x, u: x # Dynamics hs = list() # Measurement function for each camera for cam in cams: # Second camera will be moving aronud but h[1] should change with it hs.append(cam.project_point) def sim_noise(): #return (rand()-0.5)*10 return 0 # 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, hs[0](mus[0]) + sim_noise()) mus.append(mu1) Sigmas.append(Sigma1) # Discretization/search dxy = 0.1 dtheta = .314 min_trace = inf z1 = hs[1](mus[0]) + sim_noise() for x in arange(-1, dxy, 1): for y in arange(-1, dxy, 1): if x == 0 and y == 0: # Object at focal pt. exception continue for theta in arange(-pi, pi, dtheta): if theta == 0: # Results in inf/error continue