A = np.matrix([[1.2,0],
[0,1.4]])
B = np.matrix([1,100]).T
Q = np.matrix([[1,0],
[0,1]])
R = np.eye(1) * 1
T = 40
gain_schedule,cost_to_go_schedule = dtfh_lqr(A,B,Q,R,T-1)
x0 = np.array([2,1])
xsol, usol = simulate_lti_fb_dt(A,B,x0,-gain_schedule,T)
plt.figure(None)
plt.plot(xsol[0],xsol[1])
plt.plot(xsol[0],xsol[1],'o')
fig = plt.figure(None)
fig.add_subplot(3,1,1).plot(xsol[0])
fig.add_subplot(3,1,2).plot(xsol[1])
fig.add_subplot(3,1,3).plot(usol[0,:])
(Ah,Bh,Qh,Rh,pk) = AQR(A=A,B=B,c=c,Q=Q,R=R,q=q,r=r,ctdt='dt')
T = 50
x0 = np.array([0e0,0])
DUAL = False
if not DUAL:
gain_matrices,cost_to_go_matrices = dtfh_lqr(Ah,Bh,Qh,Rh,N=T-1,
Q_terminal=Qh*1e6)
else:
gain_matrices,cost_to_go_matrices = dtfh_lqr_dual(Ah,Bh,Qh,Rh,N=T-1,
Q_terminal_inv=Qh*0e0)
dp_xs,dp_us = simulate_lti_fb_dt(A=Ah,B=Bh,x0=np.concatenate([x0,[1]]),
gain_schedule=-gain_matrices,T=T)
qp_solution,(QP_P,QP_q,QP_A,QP_B),qp_xs,qp_us = LQR_QP(Ah,Bh,Qh,Rh,T=T,x0=np.concatenate([x0,[1]]),
#xT=np.concatenate([[10,0],[1]])
#xT=np.concatenate([[1,0],[1]])
xT=np.concatenate([desired.flat,[1]])
)
#qp_solution,(QP_P,QP_q,QP_A,QP_B) = LQR_QP(Ah,Bh,Qh,Rh,T=T,x0=np.concatenate([x0,[1]]))
#dsol_qp = np.array(qp_solution['x']).reshape(-1,n+m).T
#nextX = np.dot(A,dsol_qp[0:2,:]) + np.dot(B,dsol_qp[3:,:])+c
#dynamics_constraint_error = nextX[0:2,0:-1] - dsol_qp[0:2,1:]
########
#np.sum(np.abs((np.matrix(QP_A) * qp_vars)-QP_B))
import scipy import scipy.signal from lqr_tools import dtfh_lqr, simulate_lti_fb_dt import numpy as np import matplotlib.pyplot as plt A = np.matrix([[1.2, 0], [0, 1.4]]) B = np.matrix([1, 100]).T Q = np.matrix([[1, 0], [0, 1]]) R = np.eye(1) * 1 T = 40 gain_schedule, cost_to_go_schedule = dtfh_lqr(A, B, Q, R, T - 1) x0 = np.array([2, 1]) xsol, usol = simulate_lti_fb_dt(A, B, x0, -gain_schedule, T) plt.figure(None) plt.plot(xsol[0], xsol[1]) plt.plot(xsol[0], xsol[1], 'o') fig = plt.figure(None) fig.add_subplot(3, 1, 1).plot(xsol[0]) fig.add_subplot(3, 1, 2).plot(xsol[1]) fig.add_subplot(3, 1, 3).plot(usol[0, :])
