#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])
#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])))
