cost_to_go_dual_inv[i] = np.linalg.pinv(cost_to_go_dual[i])
#drive LTI CT finite-horizon LQR
n_ts = cost_to_go_dual_inv.shape[0]
n,m = lqr_dim(A,B,Q,R)
gain_schedule = np.zeros(shape=(n_ts,m,n))
for i in range(n_ts):
gain_schedule[i] = -np.dot((R.I * B.T), cost_to_go_dual_inv[i])
ts_sim = np.linspace(0,T,1000)
x0 = np.array([1,0])
desired = np.array([2,-2])
traj =simulate_lti_fb(A,B,x0=x0,ts=ts_sim,
gain_schedule=gain_schedule,
gain_schedule_ts=ts,
setpoint = desired)
print traj[-1]
plt.plot(ts_sim,traj[:,0])
plt.plot(ts_sim,traj[:,1])
if __name__=='__main__' and False:
#continuous-time affine dynamics.
# x = [vel,pos].T
d= -0e-1 #damping
g = 0 #gravity
k = 0 #spring
A = np.matrix([[d,-k],
gain_schedule_ct = np.zeros(shape=(ct_gain_samples,m,n))
for k in range(0,ct_gain_samples):
if DUAL:
ctg = np.linalg.pinv(cost_to_go_matrices_ct[k])
else:
ctg = cost_to_go_matrices_ct[k]
#a = np.dot(ctg,dsol1[:,k])
#cost_to_go[k] = np.dot(dsol1[:,k].T,a)
gain_schedule_ct[k] = -Rh.I * Bct.T * np.matrix(ctg)
traj = simulate_lti_fb(A=Act,B=Bct,x0=np.concatenate([x0,[1]]),
ts=ts,gain_schedule=gain_schedule_ct,
gain_schedule_ts=ts)
plt.figure(None)
plt.subplot(3,1,1)
#plt.plot(dsol[0,:].T,'g-') #velocity
#plt.plot(dsol[1,:].T,'b-') #position
#plt.plot(dcontrol[0,:].T,'r-') #imparted acceleration
plt.title('DP')
plt.plot(dp_xs[0,:].T,'g-') #velocity
plt.plot(dp_xs[1,:].T,'b-') #position
plt.plot(dp_us[0,:].T,'r-') #imparted acceleration
print dp_xs
plt.axhline(color='k')
#drive LTI CT finite-horizon LQR
n_ts = cost_to_go_dual_inv.shape[0]
n, m = lqr_dim(A, B, Q, R)
gain_schedule = np.zeros(shape=(n_ts, m, n))
for i in range(n_ts):
gain_schedule[i] = -np.dot((R.I * B.T), cost_to_go_dual_inv[i])
ts_sim = np.linspace(0, T, 1000)
x0 = np.array([1, 0])
desired = np.array([2, -2])
traj = simulate_lti_fb(A,
B,
x0=x0,
ts=ts_sim,
gain_schedule=gain_schedule,
gain_schedule_ts=ts,
setpoint=desired)
print traj[-1]
plt.plot(ts_sim, traj[:, 0])
plt.plot(ts_sim, traj[:, 1])
if __name__ == '__main__' and False:
#continuous-time affine dynamics.
# x = [vel,pos].T
d = -0e-1 #damping
g = 0 #gravity
k = 0 #spring
