mus[:,0] = np.mat(x0).T
Sigmas = np.zeros((Q.shape[0], Q.shape[1],T))
Sigmas[:,:,0] = np.mat(np.diag([0.0002]*num_states)) #arg
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):
mus[:,0] = np.mat(x0).T
Sigmas = np.zeros((Q.shape[0], Q.shape[1],T))
Sigmas[:,:,0] = np.mat(np.diag([0.0002]*num_states)) #arg
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):
for t in xrange(1,T):
if t < 10:
pts[:,t] = pts[:,t-1]
pts[1,t] = pts[1,t-1] + 0.05
else:
pts[:,t] = pts[:,t-1]
pts[0,t] = pts[0,t-1] - 0.05
#plt.plot(pts[0,t], pts[1,t], 'o', color='b', markersize=9)
X_bar = np.mat(np.zeros((localizer.NX, T)))
U_bar = np.mat(np.zeros((localizer.NU, T-1)))
X_bar[:, 0] = x0;
for t in xrange(1,T):
X_bar[0:2, t] = np.mat(links.inverse_kinematics(origin,pts[:,t]).ravel()).T
U_bar[0:2, t-1] = (X_bar[0:2, t] - X_bar[0:2, t-1]) / localizer.dt
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)
#newlinks = Links(np.array([-0.1, 0.1]), origin=origin, state_rep='points')
#newlinks.draw_trajectory(mat2tuple(X_bar.T))
#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
if t > 10:
U_bar[1, t-1] = 0
for t in xrange(1,T):
if t < 10:
pts[:,t] = pts[:,t-1]
pts[1,t] = pts[1,t-1] + 0.05
else:
pts[:,t] = pts[:,t-1]
pts[0,t] = pts[0,t-1] - 0.05
#plt.plot(pts[0,t], pts[1,t], 'o', color='g', markersize=9)
X_bar = np.mat(np.zeros((localizer.NX, T)))
U_bar = np.mat(np.zeros((localizer.NU, T-1)))
X_bar[:, 0] = x0;
for t in xrange(1,T):
X_bar[0:2, t] = np.mat(links.inverse_kinematics(origin,pts[:,t]).ravel()).T
U_bar[0:2, t-1] = (X_bar[0:2, t] - X_bar[0:2, t-1]) / localizer.dt
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)
#newlinks = Links(np.array([-0.1, 0.1]), origin=origin, state_rep='points')
#newlinks.draw_trajectory(mat2tuple(X_bar.T))
#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
if t > 10:
U_bar[1, t-1] = 0
