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()
links = Links(x0)
s.add_robot(links)
T = 50
X_bar = np.mat(np.zeros((links.NX, T)))
U_bar = np.mat(np.random.random_sample((links.NU, T-1)))/2
print U_bar
for t in xrange(1,T):
X_bar[:,t] = links.dynamics(X_bar[:,t-1], U_bar[:, t-1])
# Plot nominal trajectory
ax = plt.gca()
s.draw(ax=ax)
links.draw_trajectory(mat2tuple(X_bar.T))
#plt.show()
#stop
'''
X = np.mat(np.zeros((car.NX, T)))
As, Bs, Cs = car.linearize_dynamics_trajectory(X_bar, U_bar)
for t in xrange(T-1):
X[:,t+1] = As[:,:,t]*(X[:,t]-X_bar[:,t]) + Bs[:,:,t]*(U_bar[:,t]-U_bar[:,t]) +\
Cs[:,t]
s.draw()
car.draw_trajectory(mat2tuple(X.T))
plt.show()
'''
exit()
links = Links(x0)
s.add_robot(links)
T = 50
X_bar = np.mat(np.zeros((links.NX, T)))
U_bar = np.mat(np.random.random_sample((links.NU, T - 1))) / 2
print U_bar
for t in xrange(1, T):
X_bar[:, t] = links.dynamics(X_bar[:, t - 1], U_bar[:, t - 1])
# Plot nominal trajectory
ax = plt.gca()
s.draw(ax=ax)
links.draw_trajectory(mat2tuple(X_bar.T))
#plt.show()
#stop
'''
X = np.mat(np.zeros((car.NX, T)))
As, Bs, Cs = car.linearize_dynamics_trajectory(X_bar, U_bar)
for t in xrange(T-1):
X[:,t+1] = As[:,:,t]*(X[:,t]-X_bar[:,t]) + Bs[:,:,t]*(U_bar[:,t]-U_bar[:,t]) +\
Cs[:,t]
s.draw()
car.draw_trajectory(mat2tuple(X.T))
plt.show()
'''
exit()
#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])
#goal_bel[0:links.NX] = xN;
goal_bel[links.NX:] = 0
# Apply SCP
rho_bel = 0.1
rho_u = 0.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])
#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])))
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()
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5
for t in xrange(1, T):
U_bar[0, t - 1] = 0.3
U_bar[1, t - 1] = 0.1
if t > T / 2:
U_bar[0, t - 1] = 0
U_bar[1, t - 1] = -U_bar[1, t - 1]
X_bar[:, t] = car.dynamics(X_bar[:, t - 1], U_bar[:, t - 1])
# Plot nominal trajectory
ax = plt.subplot(221)
plt.title('Nominal')
s.draw(ax=ax)
car.draw_trajectory(mat2tuple(X_bar.T))
'''
X = np.mat(np.zeros((car.NX, T)))
As, Bs, Cs = car.linearize_dynamics_trajectory(X_bar, U_bar)
for t in xrange(T-1):
X[:,t+1] = As[:,:,t]*(X[:,t]-X_bar[:,t]) + Bs[:,:,t]*(U_bar[:,t]-U_bar[:,t]) +\
Cs[:,t]
s.draw()
car.draw_trajectory(mat2tuple(X.T))
plt.show()
'''
# Apply SCP
rho_x = 0.1
rho_u = 0.1
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])
#goal_bel[0:links.NX] = xN;
goal_bel[links.NX:] = 0
# Apply SCP
#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()
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5
for t in xrange(1,T):
U_bar[0,t-1] = 0.3
U_bar[1,t-1] = 0.1
if t > T / 2:
U_bar[0,t-1] = 0
U_bar[1,t-1] = -U_bar[1,t-1]
X_bar[:,t] = car.dynamics(X_bar[:,t-1], U_bar[:, t-1])
# Plot nominal trajectory
ax = plt.subplot(221)
plt.title('Nominal')
s.draw(ax=ax)
car.draw_trajectory(mat2tuple(X_bar.T))
'''
X = np.mat(np.zeros((car.NX, T)))
As, Bs, Cs = car.linearize_dynamics_trajectory(X_bar, U_bar)
for t in xrange(T-1):
X[:,t+1] = As[:,:,t]*(X[:,t]-X_bar[:,t]) + Bs[:,:,t]*(U_bar[:,t]-U_bar[:,t]) +\
Cs[:,t]
s.draw()
car.draw_trajectory(mat2tuple(X.T))
plt.show()
'''
# Apply SCP
rho_x = 0.1
