#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 Bel_bar = np.mat(np.zeros((localizer.NB, T))) for t in xrange(T): Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t]))) ''' fig = plt.gcf() s.draw_goal_state(Bel_bar[:,-1])
U_bar[0, t-1] = 1*float(t)/T U_bar[1, t-1] = 1.2 if t > 10: U_bar[1, t-1] = 0 X_bar[:,t] = localizer.dynamics(X_bar[:,t-1], U_bar[:, 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_bar[:,t-1], None) ''' # Plot nominal trajectory #ax = plt.subplot(121) ax = plt.gca() s.draw(ax=ax) localizer.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar[0:2,:], Sigmas=Sigmas[0:2,0:2,:], color='red') localizer.draw_trajectory([], mus=X_bar[2:4,0:1], Sigmas=Sigmas[2:4,2:4,0:1], color='red') localizer.draw_trajectory([], mus=X_bar[2:4,T-2:T-1], Sigmas=Sigmas[2:4,2:4,T-2:T-1], color='red') #for t in range(0,T): # localizer.mark_fov(X_bar[0:2,t], s, [-1, 1, -1, 1], color=colors[t % len(colors)]) plt.show() stop Bel_bar = np.mat(np.zeros((localizer.NB, T))) for t in xrange(T): Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t])))
U_bar[0, t-1] = 1*float(t)/T U_bar[1, t-1] = 1.2 if t > 10: U_bar[1, t-1] = 0 X_bar[:,t] = localizer.dynamics(X_bar[:,t-1], U_bar[:, 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_bar[:,t-1], None) ''' # Plot nominal trajectory ax = plt.subplot(121) ax = plt.gca() s.draw(ax=ax) localizer.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar[0:2,:], Sigmas=Sigmas[0:2,0:2,:], color='red') #localizer.draw_trajectory([], mus=X_bar[0:2,:], Sigmas=Sigmas[0:2,0:2,:], color='red') localizer.draw_trajectory([], mus=X_bar[2:4,0:1], Sigmas=Sigmas[2:4,2:4,0:1], color='red') localizer.draw_trajectory([], mus=X_bar[2:4,T-2:T-1], Sigmas=Sigmas[2:4,2:4,T-2:T-1], color='red') #ax.plot(X_bar[2,0], X_bar[3,0], 'yo') #for t in range(0,T): # localizer.mark_fov(X_bar[0:2,t], s, [-1, 1, -1, 1], color=colors[t % len(colors)]) #plt.show() #stop Bel_bar = np.mat(np.zeros((localizer.NB, T))) for t in xrange(T): Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t])))