# Setup for EKF mus = np.mat(np.zeros((num_states, T))) 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, :])
car = SimpleCar(x0) s.add_robot(car) T = 30 X_bar = np.mat(np.zeros((car.NX, T))) U_bar = np.mat(np.zeros((car.NU, T - 1))) #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))
# Generate nominal belief trajectory ''' 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):
car = SimpleCar(x0) s.add_robot(car) T = 30 X_bar = np.mat(np.zeros((car.NX, T))) U_bar = np.mat(np.zeros((car.NU, T-1))) #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()