mus[:, 0] = np.mat(x0).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.ones((car.NU, T - 1)) * 0.4
for t in xrange(1, 7):
U_bar[1, t - 1] = 0
for t in xrange(7, 10):
U_bar[1, t - 1] = 0.22
for t in xrange(10, 15):
U_bar[1, t - 1] = -0.1
for t in xrange(15, 30):
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
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], None) #NOTE No obs
# Plot nominal trajectory with covariance ellipses
ax = plt.gca()
s.draw(ax=ax)
Bel_bar = np.mat(np.zeros((car.NB, T)))
for t in xrange(T):
Bel_bar[:, t] = np.vstack((X_bar[:, t], cov2vec(Sigmas[:, :, t])))
car.draw_belief_trajectory(Bel_bar)
#plt.show()
#stop
X_bar = np.mat(np.zeros((car.NX, T))) #arg
X_bar[:,0] = np.mat(x0).T
U_bar = np.load('data/nominal_trajectory.npy')
#U_bar = np.random.random((car.NU, T-1))
#for t in xrange(1,T):
#U_bar[1,t-1] = (U_bar[1,t-1] - 0.5)
#np.save('data/nominal_trajectory', U_bar)
'''
X_bar = np.mat(d['dat_states'])
U_bar = np.mat(d['dat_ctrls'])
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
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], None) #NOTE No obs
# Plot nominal trajectory with covariance ellipses
ax = plt.gca()
s.draw(ax=ax)
#print Sigmas
#car.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:])
Bel_bar = np.mat(np.zeros((car.NB, T)))
for t in xrange(T):
Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t])))
As, Bs, Cs = car.linearize_belief_dynamics_trajectory(Bel_bar, U_bar, s, Q, R)
for t in xrange(T-1):
mus[:,0] = np.mat(x0).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()
