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[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
# 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')
