def inverseReinfLearning1(A, B, Qtrue, N):
Popt, Eopt, Kopt = mtl.care(A, B, Qtrue, 1)
Aopt = A - np.dot(B, Kopt)
Q = Qtrue * 1.1
P, E, K = mtl.care(A, B, Q, 1)
errorP = []
errorP.append(la.norm(P - Popt))
for i in range(5):
print(i)
''' 2* (P B B' P) - (A' P) - (P A) '''
Qlyap = 2 * np.dot(np.dot(P, B), np.dot(B.transpose(), P)) - np.dot(
A.transpose(), P) - np.dot(P, A)
''' making sure it is symmetric '''
Qlyap = 0.5 * (Qlyap + Qlyap.transpose())
assert ((la.eig(Qlyap)[0]) >
0).min(), 'Qlyap iterate is not positive definite!'
''' '''
P = mtl.lyap(Aopt.transpose(), Qlyap)
assert ((la.eig(P)[0]) >
0).min(), 'P iterate is not positive definite!'
errorP.append(la.norm(P - Popt))
plt.plot(errorP, 'o')
plt.grid()
plt.show()
pass
def pid_design(self):
n_x = self._A.shape[0] # number of sates
n_u = self._B.shape[1] # number of inputs
n_y = self._C.shape[0] # number of outputs
# Augmented state system (for LQR)
A_lqr = np.block([[self._A, np.zeros((n_x, n_y))],
[self._C, np.zeros((n_y, n_y))]])
B_lqr = np.block([[self._B], [np.zeros((n_y, 1))]])
# Define Q,R
Q = np.diag(self._q)
R = np.array([self._r])
# Solve for P in continuous-time algebraic Riccati equation (CARE)
#print("A_lqr shape", A_lqr.shape)
#print("Q shape",Q.shape)
(P, L, F) = cnt.care(A_lqr, B_lqr, Q, R)
if self._verbose:
print("P matrix", P)
print("Feedback gain", F)
# Calculate Ki_bar, Kp_bar
Kp_bar = np.array([F[0][0:n_x]])
Ki_bar = np.array([F[0][n_x:]])
if self._verbose:
print("Kp_bar", Kp_bar)
print("Ki_bar", Ki_bar)
# Calculate the PID kp kd gains
C_bar = np.block(
[[self._C],
[self._C.dot(self._A) - (self._C.dot(self._B)).dot(Kp_bar)]])
if self._verbose:
print("C_bar", C_bar)
print("C_bar shape", C_bar.shape)
# Calculate PID kp ki gains
kpd = Kp_bar.dot(np.linalg.inv(C_bar))
if self._verbose:
print("kpd: ", kpd, "with shape: ", kpd.shape)
kp = kpd[0][0]
kd = kpd[0][1]
# ki gain
ki = (1. + kd * self._C.dot(self._B)).dot(Ki_bar)
self._K = [kp, ki[0][0], kd]
G_plant = cnt.ss2tf(self._A, self._B, self._C, self._D)
# create PID transfer function
d_tc = 1. / 125. # Derivative time constant at nyquist freq
p_tf = self._K[0]
i_tf = self._K[1] * cnt.tf([1], [1, 0])
d_tf = self._K[2] * cnt.tf([1, 0], [d_tc, 1])
pid_tf = cnt.parallel(p_tf, i_tf, d_tf)
open_loop_tf = cnt.series(pid_tf, G_plant)
closedLoop_tf = cnt.feedback(open_loop_tf, 1)
if self._verbose:
print(" *********** PID gains ***********")
print("kp: ", self._K[0])
print("ki: ", self._K[1])
print("kd: ", self._K[2])
print(" *********************************")
return pid_tf, closedLoop_tf
def LQR(A_ss, B_Ss, Q, R):
S, L, G = cn.care(A_ss, B_ss, Q, R)
delta_u = -mtimes(G, X_est[0:4] - xbar_tilde)
u1 = np.add(ubar, delta_u)
return u1
velocity_plot = np.append(velocity_plot, X_true[3])
s_plot_est = np.append(s_plot_est, X_est[0])
n_plot_est = np.append(n_plot_est, X_est[1])
alpha_plot_est = np.append(alpha_plot_est, X_est[2])
velocity_plot_est = np.append(velocity_plot_est, X_est[3])
A_ss = integrator_jac_x_cont_a(xbar, ubar)
A_ss = A_ss[0:4, 0:4]
A_ss = np.array(A_ss)
B_ss = integrator_jac_u_cont_a(xbar, ubar)
B_ss = B_ss[0:4, 0:2]
B_ss = np.array(B_ss)
Q = np.diag([0.1, 10, 1, 2])
R = np.diag([0.5, 0.1])
S, L, G = cn.care(A_ss, B_ss, Q, R)
def plant_dyn(X_true, u, A, P_model_cov_est):
X_true = integrator_fun_a(X_true, u)
P_model_cov_true=mtimes(A, mtimes(P_model_cov_est, \
transpose(A))) + W_cov
"""
if(i<80):
X_true[4]=X_initial[4]
X_true[5]=X_initial[5]
X_true[6]=X_initial[6]
elif(i>80):
X_true[4]=-X_initial[4]
X_true[5]=-X_initial[5]
def inverseReinfLearning(A, B, Qtrue, N, p=None, debugMode=False):
Popt, Eopt, Kopt = mtl.care(A, B, Qtrue, 1)
Aopt = A - B * Kopt
Q = Qtrue * 0.9 + 0.1 * (1 - 0.5) * np.random.rand(Qtrue.shape[0],
Qtrue.shape[1])
Q = 0.5 * (Q + Q.transpose())
print(Q)
mtl.lyap(A + Aopt, Q)
P, E, K = mtl.care(A, B, Q, 1)
errorP = []
errorP.append(la.norm(P - Popt))
errorQ = []
errorQ.append(la.norm(Q - Qtrue))
errorK = []
errorK.append(la.norm(K - Kopt))
eigsP = ([], [])
eigsP[0].append(la.norm(P))
eigsP[1].append(la.norm(P, 2))
eigsQ = ([], [])
eigsQ[0].append(la.norm(Q))
eigsQ[1].append(la.norm(Q, 2))
Qi = None
for i in range(N):
print(Qi)
''' -( (P Aopt) + (Aopt' P) + ( P B B' P )) '''
target_Qi = -(np.dot(P, Aopt) + np.dot(Aopt.transpose(), P) +
la.multi_dot([P, B, B.transpose(), P]))
Qi = ((1 - p['alpha']**2)**0.5) * Qi + (
(p['alpha']**2)**0.5) * target_Qi if Qi is not None else target_Qi
Qi = 0.5 * (Qi +
Qi.transpose()) if not p['isQDiagonal'] else np.multiply(
0.5 * (Qi + Qi.transpose()), np.eye(Qi.shape[0]))
print("Q", i, " eigenvalues are: ", la.eig(Qi)[0])
if not ((la.eig(Qi)[0]) >= 0).min():
pass
assert (not debugMode) or (
(la.eig(Qi)[0]) >= 0).min(), 'Qi iterate is not positive definite!'
if p['updateP'] == 'Lyap':
Qlyap = Qi - la.multi_dot([P, B, B.transpose(), P])
Qlyap = 0.5 * (Qlyap + Qlyap.transpose())
assert (not debugMode) or (
(la.eig(Qlyap)[0]) >
0).min(), 'Qlyap iterate is not positive definite!'
P = mtl.lyap(A.transpose(), Qlyap)
assert (not debugMode) or ((
la.eig(P)[0]) > 0).min(), 'P iterate is not positive definite!'
pass
elif p['updateP'] == 'Ricc':
P, E, K = mtl.care(A, B, Qi)
assert (not debugMode) or ((
la.eig(P)[0]) > 0).min(), 'P iterate is not positive definite!'
pass
errorP.append(la.norm(P - Popt, 'fro'))
errorK.append(la.norm(K - Kopt, 'fro'))
errorQ.append(la.norm(Qi - Qtrue, 'fro'))
eigsP[0].append(la.norm(P))
eigsP[1].append(la.norm(P, 2))
eigsQ[0].append(la.norm(Qi))
eigsQ[1].append(la.norm(Qi, 2))
fig1, (ax1_1) = plt.subplots(nrows=1, ncols=1)
ax1_1.plot(errorP, 'o', label=r'$||P-\mathcal{P}||_F$', markersize=12)
ax1_1.plot(errorK, 'o', label=r'$||K-\mathcal{K}||_F$', markersize=12)
ax1_1.plot(errorQ, 'o', label=r'$||Q-\mathcal{Q}||_F$', markersize=12)
ax1_1.legend(ncol=2, loc='upper right', prop={'size': 30})
fig2, (ax2_1, ax2_2) = plt.subplots(nrows=2, ncols=1)
ax2_1.plot(eigsP[0], 'o', label=r'$||P||_F$', markersize=12)
ax2_1.plot(eigsP[1], 'o', label=r'$\lambda_{max}(P)$', markersize=12)
ax2_1.legend(ncol=1, loc='upper right', prop={'size': 30})
ax2_2.plot(eigsQ[0], 'o', label=r'$||Q||_F$', markersize=12)
ax2_2.plot(eigsQ[1], 'o', label=r'$\lambda_{max}(Q)$', markersize=12)
ax2_2.legend(ncol=1, loc='upper right', prop={'size': 30})
plt.show()
pass
], [1, 0, 0, 0], [0, 1, 0, 0]])
BQg = numpy.array([[0], [-1 / Jg], [0], [0]])
Ctot = numpy.array([[0, 1, 0, 0]])
BQa1 = numpy.array([[kp / Jr], [0], [0], [0]])
BQa2 = numpy.array([[ki / Jr], [0], [0], [0]])
print("A: ", A)
print("BQg: ", BQg)
print("Ctot: ", Ctot)
print("BQa1: ", BQa1)
print("BQa2: ", BQa2)
Q = numpy.dot(numpy.identity(4), 1e5)
print("Q: ", Q)
Rinv = 1e-5
(Pcare_t, Leig_t, Gcare_t) = cnt.care(A.transpose(), Ctot.transpose(),
Q.transpose(), Rinv)
Pcare = Pcare_t.transpose()
Gcare = Gcare_t.transpose()
print("G: ", Gcare)
df = pd.read_csv('Error.txt')
data = df.values
df2 = pd.read_csv('ErrorPosg.txt')
data2 = df2.values
df9 = pd.read_csv('ErrorPosr.txt')
data9 = df9.values
df3 = pd.read_csv('ErrorWG.txt')
data3 = df3.values
df5 = pd.read_csv('ErrorWR.txt')
data5 = df5.values
df4 = pd.read_csv('EXSOL.txt')