def lqr(A, B, Q, R):
"""
Solve the continuous time LQR controller for a continuous time system.
A and B are system matrices, describing the systems dynamics:
dx/dt = A x + B u
The controller minimizes the infinite horizon quadratic cost function:
cost = integral (x.T*Q*x + u.T*R*u) dt
where Q is a positive semidefinite matrix, and R is positive definite matrix.
Returns K, X, eigVals:
Returns gain the optimal gain K, the solution matrix X, and the closed loop system eigenvalues.
The optimal input is then computed as:
input: u = -K*x
"""
# ref Bertsekas, p.151
# first, try to solve the ricatti equation
X = np.matrix(sp_linalg.solve_continuous_are(A, B, Q, R))
# compute the LQR gain
K = np.matrix(sp_linalg.inv(R) * (B.T * X))
eigVals, eigVecs = sp_linalg.eig(A - B * K)
return K, X, eigVals
def __init__(self, settings):
# add specific private settings
settings.update(input_order=0)
settings.update(ouput_dim=1)
settings.update(input_type='system_state')
pm.Controller.__init__(self, settings)
eq_state = calc_equilibrium(settings)
# pole placement
parameter = [st.m0, st.m1, st.m2, st.l1, st.l2, st.g, self._settings["d0"], st.d1, st.d2]
self.A = symcalc.A_func(list(eq_state), parameter)
self.B = symcalc.B_func(list(eq_state), parameter)
self.C = symcalc.C
# create Q and R matrix
self.Q = np.diag(self._settings["Q"])
self.R = np.diag(self._settings["R"])
# try to solve linear riccati equation
self.P = solve_continuous_are(self.A, self.B, self.Q, self.R)
self.K = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P)
self.V = pm.calc_prefilter(self.A, self.B, self.C, self.K)
eig = np.linalg.eig(self.A - np.dot(self.B, self.K))
self._logger.info("equilibrium = " + str(eq_state))
self._logger.info("Q = " + str(self._settings["Q"]))
self._logger.info("R = " + str(self._settings["R"]))
self._logger.info("P = " + str(self.P))
self._logger.info("K = " + str(self.K))
self._logger.info("eig = " + str(eig))
self._logger.info("V = " + str(self.V[0][0]))
def calculate_controller_params(self, yaw_time_constant=None, store=True, Q=None, r=None):
if yaw_time_constant is None:
try:
yaw_time_constant = YAW_TIMECONSTANT
except:
raise()
# approximated system modell, linearized, continous calculated (seems to be fine)
A = A=array([[0, 1, 0],\
[0, -1./yaw_time_constant, 0],\
[-1, 0, 0]])
B = array([0, 1, 1])
# weigth matrices for riccati design
if Q is None:
Q = diag([1E-1, 1, 0.3])
r = ones((1,1))*30
#Q = diag([1E-1, 1, 0.3])
#r = ones((1,1))*1000
# calculating feedback
P = solve_continuous_are(A,B[:, None],Q,r)
K = sum(B[None, :] * P, axis=1)/r[0, 0]
if store:
self.KP = K[0]
self.KD = K[1]
self.KI = -K[2]
print K
return list(K)
def _test_factory(case, dec):
"""Checks if 0 = XA + A'X - XB(R)^{-1} B'X + Q is true"""
a, b, q, r, knownfailure = case
if knownfailure:
pytest.xfail(reason=knownfailure)
x = solve_continuous_are(a, b, q, r)
res = x.dot(a) + a.conj().T.dot(x) + q
out_fact = x.dot(b)
res -= out_fact.dot(solve(np.atleast_2d(r), out_fact.conj().T))
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def _test_factory(case, dec):
"""Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true"""
a, b, q, r, e, s, knownfailure = case
if knownfailure:
pytest.xfail(reason=knownfailure)
x = solve_continuous_are(a, b, q, r, e, s)
res = a.conj().T.dot(x.dot(e)) + e.conj().T.dot(x.dot(a)) + q
out_fact = e.conj().T.dot(x).dot(b) + s
res -= out_fact.dot(solve(np.atleast_2d(r), out_fact.conj().T))
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def prob5(): # Write the function find_P M, m = 23., 5. l = 4. q1, q2, q3, q4 = 1., 1., 1., 1. r = 10. tf = 60 X0 = np.array([-1, -1, .1, -.2]) A,B,Q,R = linearized_init(M,m,l,q1,q2,q3,q4,r) R_inv = inv(R) P = la.solve_continuous_are(A,B,Q,R) X,U = my_rickshaw(np.linspace(0,tf,400),X0,A,B,Q,inv(R),P)
def control(self, arm, x_des=None):
"""Generates a control signal to move the
arm to the specified target.
arm Arm: the arm model being controlled
des list: the desired system position
x_des np.array: desired task-space force,
system goes to self.target if None
"""
if self.u is None:
self.u = np.zeros(arm.DOF)
self.Q = np.zeros((arm.DOF*2, arm.DOF*2))
self.Q[:arm.DOF, :arm.DOF] = np.eye(arm.DOF) * 1000.0
self.R = np.eye(arm.DOF) * 0.001
# calculate desired end-effector acceleration
if x_des is None:
self.x = arm.x
x_des = self.x - self.target
self.arm, state = self.copy_arm(arm)
A, B = self.calc_derivs(state, self.u)
if self.solve_continuous is True:
X = sp_linalg.solve_continuous_are(A, B, self.Q, self.R)
K = np.dot(np.linalg.pinv(self.R), np.dot(B.T, X))
else:
X = sp_linalg.solve_discrete_are(A, B, self.Q, self.R)
K = np.dot(np.linalg.pinv(self.R + np.dot(B.T, np.dot(X, B))), np.dot(B.T, np.dot(X, A)))
# transform the command from end-effector space to joint space
J = self.arm.gen_jacEE()
u = np.hstack([np.dot(J.T, x_des), arm.dq])
self.u = -np.dot(K, u)
if self.write_to_file is True:
# feed recorders their signals
self.u_recorder.record(0.0, self.u)
self.xy_recorder.record(0.0, self.x)
self.dist_recorder.record(0.0, self.target - self.x)
# add in any additional signals
for addition in self.additions:
self.u += addition.generate(self.u, arm)
return self.u
def lqr(A,B,Q,R):
S = linalg.solve_continuous_are(A,B,Q,R)
K = linalg.inv(R) @ B.transpose() @ S
return K
def talker():
cmd_publisher = rospy.Publisher('/cmd_drive', cmd_drive, queue_size=200)
rospy.Subscriber("/IMU", Odometry, ImuCallback)
rospy.Subscriber("/GPS/fix", NavSatFix, retour_gps)
rospy.init_node('LQR', anonymous=True)
r = rospy.Rate(10) # 10hz
simul_time = 15 # rospy.get_param('~simulation_time', '10')
# == Dynamic Model ======================================================
# == Steady State ======================================================
A = get_model_A_matrix()
B = get_model_B_matrix()
Vyss = -(k * vit * (A[(0, 1)] * B[(1, 0)] - A[(1, 1)] * B[(0, 0)] - A[
(0, 1)] * B[(1, 1)] + A[(1, 1)] * B[(0, 1)])) / (A[(0, 0)] * B[
(1, 0)] - A[(1, 0)] * B[(0, 0)] - A[(0, 0)] * B[(1, 1)] +
A[(1, 0)] * B[(0, 1)])
Vpsiss = vit * k
eyss = 0
epsiss = -Vyss / vit
bfss = -(k * vit * (A[(0, 0)] * A[(1, 1)] - A[(0, 1)] * A[
(1, 0)])) / (A[(0, 0)] * B[(1, 0)] - A[(1, 0)] * B[(0, 0)] -
A[(0, 0)] * B[(1, 1)] + A[(1, 0)] * B[(0, 1)])
brss = -bfss
xsss = np.array([[Vyss], [Vpsiss], [eyss], [epsiss]])
usss = np.array([[bfss], [brss]])
# == LQR control (Gains) ======================================================
Q = np.array([[0.0001, 0, 0, 0], [0, 0.0001, 0, 0], [0, 0, 0.0001, 0],
[0, 0, 0, 0.0001]])
R = np.array([[20000, 0], [0, 20000]])
C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
P = solve_continuous_are(A, B, Q, R)
#print(P)
G = np.linalg.inv(R).dot(np.transpose(B)).dot(P)
#print(G)
#np.array([[-0.5,4.3e+13],[-0.5,-4.2e+13]])#
M = pinv(C.dot(pinv(A - B.dot(G))).dot(B))
#print(M)
# == Observer ======================================================
xhat = np.array([[0], [0], [0], [0]])
#print(xhat)
Gammax = 0.01 * eye(4, 4)
dt = 0.01 #periode d'échantionnage
Gammaalpha = dt * 0.00001 * eye(4, 4)
Gammabeta = dt**2 * eye(2, 2) * 0.00001
Psiref = Psi #-52*pi/180
Psipref = 0 * pi / 180
vyref = 0
xref = X
yref = Y
t0 = rospy.get_time()
file = open("steerLQR.txt", "w")
cmd = cmd_drive()
cmd.linear_speed = vit
while not rospy.is_shutdown():
print(rospy.get_time() - t0)
ey = -sin(Psiref) * (X - xref) + cos(Psiref) * (Y - yref)
epsi = (Psi - Psiref) # par rapport au nord magntiquae
#vy=-Xp*sin(Psi)+Yp*cos(Psi)
#print(epsi*180/pi,ey)
vy = xhat[0, 0]
x = np.array([[vy], [Psip], [ey], [epsi]])
yd = np.array([[vyref], [Psipref]])
y = C.dot(x)
#print('u',u)
u = M.dot(yd) - G.dot(x)
xhat, Gammax = kalman(xhat, Gammax, dt * B.dot(u), y, Gammaalpha,
Gammabeta,
eye(4, 4) + dt * A, C)
cmd.linear_speed = vit
cmd.steering_angle_front = u[0, 0]
cmd.steering_angle_rear = u[1, 0]
cmd_publisher.publish(cmd)
#file.write(' '.join((str(rospy.get_time()-t0), str(u[0,0]), str(u[1,0]))))
print(u)
print(rospy.get_time() - t0)
r.sleep()
file.close()
print(rospy.get_time() - t0)
linear_state_matrix, linear_input_matrix, inputs = \
kane.linearize(new_method=True, A_and_B=True)
f_A_lin = linear_state_matrix.subs(parameter_dict).subs(equilibrium_dict)
f_B_lin = linear_input_matrix.subs(parameter_dict).subs(equilibrium_dict)
m_mat = mass_matrix.subs(parameter_dict).subs(equilibrium_dict)
A = matrix(m_mat.inv() * f_A_lin).astype(float)
B = matrix(m_mat.inv() * f_B_lin).astype(float)
assert controllable(A, B)
Q = matrix(eye(6))
R = matrix(eye(3))
S = solve_continuous_are(A, B, Q, R)
K = inv(R) * B.T * S
# This is an annoying little issue. We specified the order of things when
# creating the rhs function, but the linearize function returns the F_B
# matrix in the order corresponding to whatever order it finds the joint
# torques. This would also screw things up if we specified a different
# ordering of the coordinates and speeds as the standard kane._q + kane._u
K = K[[0, 2, 1], :]
def controller(x, t):
return -asarray(dot(K, x)).flatten()
def check_case(self, a, b, q, r):
"""Checks if (A'X + XA - XBR^-1B'X+Q=0) is true"""
x = solve_continuous_are(a, b, q, r)
assert_array_almost_equal(
a.getH()*x + x*a - x*b*inv(r)*b.getH()*x + q, 0.0)
'''
If you are confident in the measures, increase
the confidence so that measure_confidence >> 1.
Otherwise, make it << 1.
'''
measure_confidence = 1
Aext = np.mat('[0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 0 0]')
Cext = np.mat('[1 0 0 0; 0 0 1 0]')
n = Aext.shape[0]
ny = Cext.shape[0]
G = np.identity(n);
Q = measure_confidence * np.identity(n);
R = np.identity(ny);
P = solve_continuous_are(Aext.T, Cext.T, Q, R)
L = P * Cext.T * R;
dt = .1
A = np.identity(n) + dt * (Aext - L * Cext)
B = L * dt
C = Cext
xest_k = np.mat('[0; 0; 0; 0]')
with pymorse.Morse() as morse:
while True:
def talker():
cmd_publisher= rospy.Publisher('cmd_drive', cmd_drive,queue_size=10)
data_pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
rospy.Subscriber("/IMU", Odometry, ImuCallback)
rospy.Subscriber("/GPS/fix",NavSatFix,retour_gps)
rospy.init_node('LQR', anonymous=True)
r = rospy.Rate(200) # 10hz
simul_time = rospy.get_param('~simulation_time', '10')
# == Dynamic Model ======================================================
# == Steady State ======================================================
B=get_model_B_matrix()
# == LQR control (Gains) ======================================================
Q=np.array([ [0.0001, 0,0, 0],
[0,0.0001, 0, 0],
[0,0, 0.0001, 0],
[0,0,0, 0.0001]])
R=np.array([ [2000,0],
[0,2000]])
C=np.array([[1, 0, 0 ,0],
[0, 1, 0 ,0]])
#print(M)
# == Observer ======================================================
#print(xhat)
Gammax=0.01*eye(4,4)
dt=0.01 #periode d'échantionnage
Gammaalpha=dt*0.00001*eye(4,4)
Gammabeta=dt**2*eye(2,2)*0.00001
mat=np.loadtxt('/home/summit/Spido_ws/recorded_files/mat.txt')
#mat[:,1]=mat[:,1]-mat[0,1]+X
#mat[:,2]=mat[:,2]-mat[0,2]+Y
Psiref=mat[0,3]
Psipref=mat[0,6]
xref=mat[0,1]
yref=mat[0,2]
ey=-sin(Psiref)*(X-xref)+cos(Psiref)*(Y-yref)
epsi=(Psi-Psiref)
xhat=np.array([[0],[Psip],[ey],[epsi]])
#print(mat)
#mat[:,3]=mat[:,3]-mat[0,3]+Psi
#coords=mat[:,1:3]
line = geom.LineString(mat[:,1:3])
t0= rospy.get_time()
#pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
#file = open("steerLQR.txt","w")
while (rospy.get_time()-t0<=simul_time):
cmd=cmd_drive()
#subprocess.check_call("rosservice call /gazebo/pause_physics", shell=True)
point = geom.Point(X,Y)
nearest_pt=line.interpolate(line.project(point))
distance,index = spatial.KDTree(mat[:,1:3]).query(nearest_pt)
xref=mat[index,1]
yref=mat[index,2]
Psiref=mat[index,3]
k=mat[index,7]
vyref=mat[index,5]
Psipref=mat[index,6]
ey=-sin(Psiref)*(X-xref)+cos(Psiref)*(Y-yref)
epsi=(Psi-Psiref)
#vy=-Xp*sin(Psi)+Yp*cos(Psi)
vy=xhat[0,0]
x=np.array([[vy],[Psip],[ey],[epsi]])
yd=np.array([[vyref],[Psipref]])
y=C.dot(x)
#print('u',u)
A=get_model_A_matrix(k)
Vyss=-(k*vit*(A[(0,1)]*B[(1,0)] - A[(1,1)]*B[(0,0)] - A[(0,1)]*B[(1,1)] + A[(1,1)]*B[(0,1)]))/(A[(0,0)]*B[(1,0)] - A[(1,0)]*B[(0,0)] - A[(0,0)]*B[(1,1)] + A[(1,0)]*B[(0,1)])
Vpsiss=vit*k
eyss=0;
epsiss=-Vyss/vit;
bfss=-(k*vit*(A[(0,0)]*A[(1,1)] - A[(0,1)]*A[(1,0)]))/(A[(0,0)]*B[(1,0)] - A[(1,0)]*B[(0,0)] - A[(0,0)]*B[(1,1)] + A[(1,0)]*B[(0,1)])
brss=-bfss;
xsss=np.array([[Vyss],[Vpsiss],[eyss],[epsiss]])
usss=np.array([[bfss],[brss]])
P=solve_continuous_are(A, B, Q, R)
#print(P)
G=np.linalg.inv(R).dot(np.transpose(B)).dot(P)
#print(G)
#np.array([[-0.5,4.3e+13],[-0.5,-4.2e+13]])#
M = pinv(C.dot(pinv(A-B.dot(G))).dot(B))
u=M.dot(yd)-G.dot(x-xsss)+usss
#u=M.dot(yd)-G.dot(x)
#print(u)
xhat,Gammax=kalman(xhat,Gammax,dt*B.dot(u),y,Gammaalpha,Gammabeta,eye(4,4)+dt*A,C)
#print('xhat',xhat)
#vy1=-Xp*sin(Psi)+Yp*cos(Psi);# // vy:vitesse latérale expriméé dans le repère véhicule
#;//-xpref*sin(psiref)+ypref*cos(psiref); // vy:vitesse latérale expriméé dans le repère véhicule
#subprocess.check_call("rosservice call /gazebo/unpause_physics", shell=True)
#unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
cmd.linear_speed=vit
cmd.steering_angle_front=u[0,0]
cmd.steering_angle_rear=u[1,0]
cmd_publisher.publish(cmd)
posture = np.array([X,Y,Psi,ey,epsi,vy,Psip,u[0,0],u[1,0]], dtype=np.float32)
data_pub.publish(posture)
#file.write(' '.join((str(rospy.get_time()-t0), str(u[0,0]), str(u[1,0]))))
r.sleep()
cmd.steering_angle_front=0
cmd.steering_angle_rear=0
aa=vit
while (aa>0):
cmd=cmd_drive()
aa=aa-0.1
if aa<0:
aa=0
cmd.linear_speed=aa
cmd_publisher.publish(cmd)
# initial state x0 = np.array([[0.98], [0], [0.2], [0]]) # size of A and B n1 = np.size(A,0) n2 = np.size(B,1) # Solve the continuous time lqr controller # to get controller gain 'K' # J = x'Qx + u'Ru # A'X + XA - XBR^(-1)B'X + Q = 0 # K = inv(R)B'X Q = C.T.dot(C) R = np.eye(n2) X = np.matrix(lin.solve_continuous_are(A, B, Q, R)) K = -np.matrix(lin.inv(R)*(B.T*X)).getA() # observability ob = obsv(A, C) print "observability =", ob # controllability cb = ctrb(A, B) print "controllability =", cb # get the observer gain, using pole placement p = np.array([-13, -12, -11, -10]) fsf1 = sgn.place_poles(A.T, C.T, p) L = fsf1.gain_matrix.T
def solve_lqr(A, B, Q, R):
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P
# K,S,E=control.lqr(A,B,Q,R)
return K
def talker():
cmd_publisher = rospy.Publisher('cmd_drive', cmd_drive, queue_size=10)
rospy.Subscriber("/IMU", Odometry, ImuCallback)
rospy.Subscriber("/GPS/fix", NavSatFix, retour_gps)
rospy.init_node('LQR', anonymous=True)
r = rospy.Rate(10) # 10hz
simul_time = rospy.get_param('~simulation_time', '10')
# == Dynamic Model ======================================================
from std_srvs.srv import Empty
# == Steady State ======================================================
B = get_model_B_matrix()
# == LQR control (Gains) ======================================================
Q = np.array([[0.0001, 0, 0, 0], [0, 0.0001, 0, 0], [0, 0, 0.0001, 0],
[0, 0, 0, 0.0001]])
R = np.array([[20000, 0], [0, 20000]])
C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
#print(M)
# == Observer ======================================================
xhat = np.array([[0], [0], [1.5], [0]])
#print(xhat)
Gammax = 0.01 * eye(4, 4)
dt = 0.01 #periode d'échantionnage
Gammaalpha = dt * 0.00001 * eye(4, 4)
Gammabeta = dt**2 * eye(2, 2) * 0.00001
Psiref = 0 * pi / 180
Psipref = 0 * pi / 180
vyref = 0
mat = np.loadtxt('/home/bachir/spido_ws/src/spido_riding/scripts/mat.txt')
mat[:, 1] = mat[:, 1] - mat[0, 1] + X
mat[:, 2] = mat[:, 2] - mat[0, 2] + Y
mat[:, 3] = mat[:, 3] - mat[0, 0] + Psi
coords = mat[:, 1:3]
line = geom.LineString(coords)
t0 = rospy.get_time()
#pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
file = open("steerLQR.txt", "w")
while (rospy.get_time() - t0 <= simul_time):
cmd = cmd_drive()
#subprocess.check_call("rosservice call /gazebo/pause_physics", shell=True)
point = geom.Point(X, Y)
nearest_pt = line.interpolate(line.project(point))
distance, index = spatial.KDTree(coords).query(nearest_pt)
xref = coords[index, 0]
yref = coords[index, 1]
Psiref = mat[index, 3]
ey = -sin(Psiref) * (X - xref) + cos(Psiref) * (Y - yref)
epsi = (Psi - Psiref)
#vy=-Xp*sin(Psi)+Yp*cos(Psi)
vy = xhat[0, 0]
x = np.array([[vy], [Psip], [ey], [epsi]])
yd = np.array([[vyref], [Psipref]])
y = C.dot(x)
#print('u',u)
k = mat[index, 7]
A = get_model_A_matrix(k)
Vyss = -(k * vit * (A[(0, 1)] * B[(1, 0)] - A[(1, 1)] * B[(0, 0)] - A[
(0, 1)] * B[(1, 1)] + A[(1, 1)] * B[
(0, 1)])) / (A[(0, 0)] * B[(1, 0)] - A[(1, 0)] * B[(0, 0)] -
A[(0, 0)] * B[(1, 1)] + A[(1, 0)] * B[(0, 1)])
Vpsiss = vit * k
eyss = 0
epsiss = -Vyss / vit
bfss = -(k * vit * (A[(0, 0)] * A[(1, 1)] - A[(0, 1)] * A[
(1, 0)])) / (A[(0, 0)] * B[(1, 0)] - A[(1, 0)] * B[(0, 0)] -
A[(0, 0)] * B[(1, 1)] + A[(1, 0)] * B[(0, 1)])
brss = -bfss
xsss = np.array([[Vyss], [Vpsiss], [eyss], [epsiss]])
usss = np.array([[bfss], [brss]])
P = solve_continuous_are(A, B, Q, R)
#print(P)
G = np.linalg.inv(R).dot(np.transpose(B)).dot(P)
#print(G)
#np.array([[-0.5,4.3e+13],[-0.5,-4.2e+13]])#
M = pinv(C.dot(pinv(A - B.dot(G))).dot(B))
u = M.dot(yd) - G.dot(x - xsss) + usss
xhat, Gammax = kalman(xhat, Gammax, dt * B.dot(u), y, Gammaalpha,
Gammabeta,
eye(4, 4) + dt * A, C)
#print('xhat',xhat)
vy1 = -Xp * sin(Psi) + Yp * cos(Psi)
# // vy:vitesse latérale expriméé dans le repère véhicule
#;//-xpref*sin(psiref)+ypref*cos(psiref); // vy:vitesse latérale expriméé dans le repère véhicule
#subprocess.check_call("rosservice call /gazebo/unpause_physics", shell=True)
#unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
cmd.linear_speed = vit
cmd.steering_angle_front = u[0, 0]
cmd.steering_angle_rear = u[1, 0]
cmd_publisher.publish(cmd)
file.write(' '.join(
(str(rospy.get_time() - t0), str(u[0, 0]), str(u[1, 0]))))
r.sleep()
#rospy.loginfo("out of the loop")
file.close()
def run_step(self):
A = np.matrix([[0, self.v_target * (5. / 18.)], [0, 0]])
B = np.matrix([[0], [(self.v_target / self.wheelbase) * (5. / 18.)]])
V = np.matrix(linalg.solve_continuous_are(A, B, self.Q, self.R))
K = np.matrix(linalg.inv(self.R) * (B.T * V))
return K
# Q[10:13,10:13] = 1*np.eye(3)
R = 1e0 * np.eye(4)
R[0, ]
Aout = np.zeros((14, 14))
Aout[0:3, 3:6] = np.eye(3)
Aout[3:6, 6:9] = np.eye(3)
Aout[6:9, 10:13] = np.eye(3)
Aout[9, 13] = 1
Bout = np.zeros((14, 4))
Bout[10:14, 0:4] = np.eye(4)
print("A:", Aout, "B:", Bout)
Mout = solve_continuous_are(Aout, Bout, Q, R)
# print("M_out:", Mout)
# #-[2] Linearization and get (K,S) for LQR
# A, B =robobee_plantBS.GetLinearizedDynamics(uf, xf)
# print("A:", A, "B:", B)
# ControllabilityMatrix = np.zeros((15, 4*15))
# for i in range(0,15):
# if i==0:
# TestAB = B
# ControllabilityMatrix= TestAB
# # print("ControllabilityMatrix:", ControllabilityMatrix)
# else:
def LQR(v_target, wheelbase, Q, R): A = np.matrix([[0, v_target*(5./18.)], [0, 0]]) B = np.matrix([[0], [(v_target/wheelbase)*(5./18.)]]) V = np.matrix(linalg.solve_continuous_are(A, B, Q, R)) K = np.matrix(linalg.inv(R)*(B.T*V)) return K
def compute_LQR_gain(A, B, Q, R):
P = sl.solve_continuous_are(A, B, Q, R)
gain = sl.inv(R) @ B.T @ P
return gain
def lqr(A, B, Q, R):
X = solve_continuous_are(A, B, Q, R)
K = inv(R) @ (B.T @ X)
eigVals, eigVecs = eig(A - B * K)
return K
def sdre():
while not rospy.is_shutdown():
global x, y, z, roll, pitch, yaw, vel_rover, goal, goal_body, v_x, v_y, v_z, Rot_body_to_inertial, Rot_inertial_to_body, yaw2, acc_rover, v3, zhold, change_landing, land_flag, bot_no, z_init, t, t_p
t = timer()
#### Global to Body conversion for the goal
posn.write('%f;' % float(x))
posn.write('%f;' % float(y))
posn.write('%f;' % float(z))
posn.write('%f;' % float(v_x))
posn.write('%f;' % float(v_y))
posn.write('%f;' % float(v_z))
# if land_flag!=[]:
# if land_flag[bot_no] == 0:
# goal[2] = z
# if z>5 and bot_no!=0:
# goal[0] = goal[0] - 3*cos(yaw2)*(1/(1+10*exp(-(z-5))))
# goal[1] = goal[1] - 3*sin(yaw2)*(1/(1+10*exp(-(z-5))))
# if bot_no == 0 and change_landing!=100:
# goal_body[0] = goal[0]
# goal_body[1] = goal[1]
# goal_body[2] = goal[2]
# else:
goal[0] = 10 * cos(0.1 * (t - t_p))
goal[1] = 10 * sin(0.1 * (t - t_p))
goal[2] = 10 + 0.15 * (t - t_p)
if goal[2] > 50:
goal[2] = 50
vel_rover[0] = -1 * sin(0.1 * (t - t_p))
vel_rover[1] = 1 * cos(0.1 * (t - t_p))
vel_rover[2] = 0.15
if goal[2] >= 50:
vel_rover[2] = 0
goal_body[0] = goal[0] - x
goal_body[1] = goal[1] - y
goal_body[2] = goal[2] - z
posn.write('%f;' % float(goal[0]))
posn.write('%f;' % float(goal[1]))
posn.write('%f;' % float(goal[2]))
posn.write('%f;' % float(vel_rover[0]))
posn.write('%f;' % float(vel_rover[1]))
posn.write('%f;' % float(vel_rover[2]))
posn.write('\n')
print('goal', goal)
goal_body = np.dot(Rot_inertial_to_body, goal_body.transpose())
# if (sqrt(goal_body[0]**2 + goal_body[1]**2)>0.6 and change_landing!=100 and land_flag[bot_no]==1) or (detect==0 and bot_no==0 and land_flag[bot_no==1]):
# if sqrt(goal[0]**2+goal[1]**2)<z:
# goal[2] = sqrt(goal[0]**2+goal[1]**2)
#### Weighting Matrices Q R
# if bot_no == 0:
# Q = np.array([[((5*goal_body[0])**2)/abs(goal_body[2]+0.0001)+1, 0, 0, 0, 0, 0]
# ,[0, abs(150*(0.5+abs(goal_body[2]))*(vel_rover[0]-v_x)/(0.001+0.1*abs(goal_body[0]))), 0, 0, 0, 0]
# ,[0, 0, ((5*goal_body[1])**2)/abs(goal_body[2]+0.0001)+1, 0, 0, 0]
# ,[0, 0, 0, abs(150*(0.5+abs(goal_body[2]))*(vel_rover[1]-v_y)/(0.001+0.1*abs(goal_body[1]))), 0, 0]
# ,[0, 0, 0, 0, 1+(10*goal_body[2]/sqrt(0.01+0.01*(goal_body[0]**2)+0.01*(goal_body[1]**2)))**2, 0]
# ,[0, 0, 0, 0, 0, 1/abs(goal_body[2]+0.001)]])
Q = np.array([
[((15 * goal_body[0])**2) / abs(goal_body[2] + 0.0001) +
50 / abs(abs(goal_body[0] + 0.00001) - 1) +
100 / abs(goal_body[0] + 0.00001), 0, 0, 0, 0, 0, 0],
[
0,
abs(200 * (0.5 + abs(goal_body[2])) * (vel_rover[0] - v_x) /
(0.001 + 0.01 * abs(goal_body[0] + 0.00001))), 0, 0, 0, 0,
0
],
[
0, 0, ((15 * goal_body[1])**2) / abs(goal_body[2] + 0.0001) +
50 / abs(abs(goal_body[1] + 0.00001) - 1) +
100 / abs(goal_body[0] + 0.00001), 0, 0, 0, 0
],
[
0, 0, 0,
abs(200 * (0.5 + abs(goal_body[2])) * (vel_rover[1] - v_y) /
(0.001 + 0.01 * abs(goal_body[1]))), 0, 0, 0
],
[
0, 0, 0, 0,
1 + (10 * goal_body[2] / sqrt(0.01 + 0.01 *
(goal_body[0]**2) + 0.01 *
(goal_body[1]**2)))**2, 0, 0
] #normal
#,[0, 0, 0, 0, 1+((10*(goal_body[2]*10/z_init)+10*(change_landing))/sqrt(0.01+0.01*(goal_body[0]**2)+0.01*(goal_body[1]**2)))**2, 0, 0] #alt hold
,
[0, 0, 0, 0, 0, 1 / abs(goal_body[2] + 0.001), 0] #normal
#,[0, 0, 0, 0, 0, (1-change_landing+0.0001)/abs((goal_body[2]*10/z_init)+0.001), 0] #alt hold
,
[
0, 0, 0, 0, 0, 0,
0.001 / abs((goal_body[2] * z_init / 10) + 0.001)
]
])
R = np.array([
[800, 0, 0, 0] #z - accn
,
[0, 75000, 0, 0] #Pitch
,
[0, 0, 75000, 0] #Roll
,
[0, 0, 0, 2]
])
# else:
# Q = np.array([[((5*goal_body[0])**2)/abs(0.05*goal_body[2]**2+0.0001)+1, 0, 0, 0, 0, 0, 0]
# ,[0, abs(150*(vel_rover[0]-v_x)/(0.001+0.01*abs(goal_body[0])+0.05*abs(goal_body[2]))), 0, 0, 0, 0, 0]
# ,[0, 0, ((5*goal_body[1])**2)/abs(0.05*goal_body[2]**2+0.0001)+1, 0, 0, 0, 0]
# ,[0, 0, 0, abs(150*(vel_rover[1]-v_x)/(0.001+0.01*abs(goal_body[1])+0.05*abs(goal_body[2]))), 0, 0, 0]
# ,[0, 0, 0, 0, 1+((30*goal_body[2])/sqrt(0.01+0.1*(goal_body[0]**2)+0.1*(goal_body[1]**2)))**2, 0, 0] #normal
# #,[0, 0, 0, 0, 1+((10*goal_body[2]+10*(land))/sqrt(0.01+0.01*(goal_body[0]**2)+0.01*(goal_body[1]**2)))**2, 0, 0] #alt hold
# ,[0, 0, 0, 0, 0, 1/abs(goal_body[2]+0.001), 0] #normal
# #,[0, 0, 0, 0, 0, (1-land+0.0001)/abs(goal_body[2]+0.001), 0] #alt hold
# ,[0, 0, 0, 0, 0, 0, 10/abs(goal_body[2]+0.001)]])
#
# R = np.array([[100, 0, 0, 0] #z - accn
# ,[0, 100, 0, 0] #Pitch
# ,[0, 0, 100, 0] #Roll
# ,[0, 0, 0, 500]])
#### Calculation for control done in body fixed frame
### d2(e_x)/dt2 = 0-d2(x)/dt2 so all signs inverted
#if bot_no!=0:
X = np.array([[goal_body[0]], [vel_rover[0] - v_x], [goal_body[1]],
[vel_rover[1] - v_y], [goal_body[2]],
[vel_rover[2] - v_z], [yaw2 - yaw]])
#else:
# X = np.array([[goal_body[0]],[vel_rover[0]],[goal_body[1]],[vel_rover[1]],[goal_body[2]],[vel_rover[2]],[yaw2-yaw]])
B = np.array([[0, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]])
P = la.solve_continuous_are(A, B, Q, R)
u = np.dot(-np.linalg.inv(R), B.transpose())
u = np.dot(u, P)
u = np.dot(u, X)
u0 = float(u[0])
u1 = float(u[1])
u2 = float(u[2])
u3 = float(u[3])
#### Normalizing the received thrust
u0 = ((acc_rover[2] - u0) * 1.5 + 14.7) / 29.4
u1 = (acc_rover[0] - u1) / 9.8
u2 = (u2 - acc_rover[1]) / 9.8
u3 = v3 - u3
#### Restrict rotation angles to 10 deg
if u0 > 1:
u0 = 1
if u0 < 0:
u0 = 0
if u1 > 10 * np.pi / 180:
u1 = 10 * np.pi / 180
if u1 < -10 * np.pi / 180:
u1 = -10 * np.pi / 180
if u2 > 10 * np.pi / 180:
u2 = 10 * np.pi / 180
if u2 < -10 * np.pi / 180:
u2 = -10 * np.pi / 180
#### Start descending for small errors
# if sqrt(goal_body[0]**2+goal_body[1]**2)<0.8 and abs(goal_body[2])<1 and z<1 and abs(goal[2])<0.5 and change_landing!=100:
# #change_landing = 100
# rospy.loginfo("LAND")
# u0 = 0.0
# u1 = 0.0
# u2 = 0.0
# if sqrt(goal_body[0]**2+goal_body[1]**2)<0.8 and abs(goal_body[2])<0.2 and z<1 and abs(goal[2])<0.5:
# change_landing=100
# if change_landing==100 and z<1:
# armService = rospy.ServiceProxy('drone'+str(bot_no)+'/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
# armService(True)
# takeoffService = rospy.ServiceProxy('drone'+str(bot_no)+'/mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL)
# takeoffService(altitude = 1)
#### Convert to quaternions and publish
quater = tf.transformations.quaternion_from_euler(
u2, u1, yaw + np.pi / 2) #0
msg.header = Header()
msg.type_mask = 0
msg.orientation.x = quater[0]
msg.orientation.y = quater[1]
msg.orientation.z = quater[2]
msg.orientation.w = quater[3]
msg.body_rate.x = 0.0
msg.body_rate.y = 0.0
msg.body_rate.z = u3
msg.thrust = u0
pub.publish(msg)
rate = rospy.Rate(50)
rate.sleep
print("\n\n1. Set point is not a fixed point")
#-[2] Linearization and get (K,S) for LQR
A, B = robobee_plant.GetLinearizedDynamics(uf, xf)
# print("A:", A, "B:", B)
Q = 1 * np.eye(13)
Q[0:3, 0:3] = 10 * np.eye(3)
Q[10:13, 10:13] = 1 * np.eye(3)
R = np.eye(4)
N = np.zeros((13, 4))
M_lqr = solve_continuous_are(A, B, Q, R)
# print("M_lqr:", M_lqr)
K_py = np.dot(np.dot(np.linalg.inv(R), B.T), M_lqr)
print("K_py", K_py)
# print("K_py size:", K_py.shape)
K_, S_ = LinearQuadraticRegulator(A, B, Q, R)
# print("K_:", K_, "S_:", S_)
ControllabilityMatrix = np.zeros((13, 4 * 13))
for i in range(0, 13):
if i == 0:
TestAB = B
ControllabilityMatrix = TestAB
# print("ControllabilityMatrix:", ControllabilityMatrix)
def solve_ricc_lrcf(A, E, B, C, R=None, S=None, trans=False, options=None):
"""Compute an approximate low-rank solution of a Riccati equation.
See :func:`pymor.algorithms.riccati.solve_ricc_lrcf` for a general
description.
This function uses `scipy.linalg.solve_continuous_are`, which
is a dense solver.
Therefore, we assume all |Operators| and |VectorArrays| can be
converted to |NumPy arrays| using
:func:`~pymor.algorithms.to_matrix.to_matrix` and
:func:`~pymor.vectorarrays.interfaces.VectorArrayInterface.to_numpy`.
Parameters
----------
A
The |Operator| A.
E
The |Operator| E or `None`.
B
The operator B as a |VectorArray| from `A.source`.
C
The operator C as a |VectorArray| from `A.source`.
R
The operator R as a 2D |NumPy array| or `None`.
S
The operator S as a |VectorArray| from `A.source` or `None`.
trans
Whether the first |Operator| in the Riccati equation is
transposed.
options
The solver options to use (see :func:`ricc_lrcf_solver_options`).
Returns
-------
Z
Low-rank Cholesky factor of the Riccati equation solution,
|VectorArray| from `A.source`.
"""
_solve_ricc_check_args(A, E, B, C, R, S, trans)
options = _parse_options(options, ricc_lrcf_solver_options(), 'scipy',
None, False)
if options['type'] != 'scipy':
raise ValueError(
f"Unexpected Riccati equation solver ({options['type']}).")
A_source = A.source
A = to_matrix(A, format='dense')
E = to_matrix(E, format='dense') if E else None
B = B.to_numpy().T
C = C.to_numpy()
S = S.to_numpy().T if S else None
if R is None:
R = np.eye(C.shape[0] if not trans else B.shape[1])
if not trans:
if E is not None:
E = E.T
X = solve_continuous_are(A.T, C.T, B.dot(B.T), R, E, S)
else:
X = solve_continuous_are(A, B, C.T.dot(C), R, E, S)
return A_source.from_numpy(_chol(X).T)
K1 = K1.reshape(n, n) + 1e-14 * np.eye(n)
K2 = K2.reshape(n, n) + 1e-14 * np.eye(n)
Mean1 = Mean1.reshape(n, 1)
Mean2 = Mean2.reshape(n, 1)
# K1 = k1.compute_K(x_obs, x_obs) + 1e-14*np.eye(n)
# K2 = k2.compute_K(x_obs, x_obs) + 1e-14*np.eye(n)
# Mean1 = np.array([0 for i in x_obs]).reshape(n, 1)
# Mean2 = np.array([0 for i in x_obs]).reshape(n, 1)
# Find the euclidean barycenter
meuclid, Keuclid = (Mean1 + Mean2) / 2, (K1 + K2) / 2
# Find the wasserstein barycenter
# Solve.
print('Computing...')
C1 = spa.solve_continuous_are(np.zeros((n, n)), np.linalg.cholesky(K1), K2,
np.eye(n))
print('Symdiff C1:', np.sum(np.abs(C1 - C1.T)))
print('Mineig C1:', np.min(np.real(np.linalg.eig(C1)[0])))
iA1 = (np.eye(n) + C1) / 2.0
K_from1 = iA1 @ K1 @ iA1.T
K = K_from1
print('Checking...')
print('L1 norm of FP difference:', np.sum(np.abs(fp_difference(K, K1, K2))))
mw, Kw = (Mean1 + Mean2) / 2, K
# Compare the two barycenters
plot(Mean2, K2, x_obs)
plot(Mean1, K1, x_obs)
plot(mw, Kw, x_obs)
# plot(mw, Kw, x_obs)
def Cqp(omega_0, Ap, Dp):
"""Given the free particle Ap and Dp matrices and the frequency of the harmonic oscillator, computes the full covariance matrix."""
dAqp = Aqp(omega_0, Ap)
dDqp = Dqp(omega_0, Dp)
return sp.solve_continuous_are(-dAqp, np.zeros(dAqp.shape), dDqp,
np.eye(dAqp.shape[-1]))
# initial state x0 = np.array([[0.98], [0], [0.2], [0]]) # size of A and B n1 = np.size(A, 0) n2 = np.size(B, 1) # Solve the continuous time lqr controller # to get controller gain 'K' # J = x'Qx + u'Ru # A'X + XA - XBR^(-1)B'X + Q = 0 # K = inv(R)B'X Q = C.T.dot(C) R = np.eye(n2) X = np.matrix(lin.solve_continuous_are(A, B, Q, R)) K = -np.matrix(lin.inv(R) * (B.T * X)).getA() # observability ob = obsv(A, C) print "observability =", ob # controllability cb = ctrb(A, B) print "controllability =", cb # get the observer gain, using pole placement p = np.array([-13, -12, -11, -10]) fsf1 = sgn.place_poles(A.T, C.T, p) L = fsf1.gain_matrix.T
def lqr(self, A, B, Q, R):
P = np.matrix(sp_linalg.solve_continuous_are(A, B, Q, R)) #solve continous time ricatti equation
K = np.matrix(sp_linalg.inv(R)*(B.T*P)) #compute the LQR gain
eigVals, eigVecs = sp_linalg.eig(A-B*K)
return K, eigVals
np.abs(np.real(linalg.eig(Ac, left=False, right=False,
check_finite=False))))
dT = 1 / (2 * evals[-1])
Tsim = (8 / np.min(evals[~np.isclose(evals[np.nonzero(evals)], 0)])
if np.sum(np.isclose(evals[np.nonzero(evals)], 0)) > 0 else 8)
Ad, Bd, Cd, Dd, dT = signal.cont2discrete((Ac, Bc, Cc, Dc), dT)
dt_sys = LTISystem(Ad, Bd, Cd, dt=dT)
dt_sys.initial_condition = ic
Q = np.eye(Ac.shape[0])
R = np.eye(Bc.shape[1] if len(Bc.shape) > 1 else 1)
Sc = linalg.solve_continuous_are(
Ac,
Bc,
Q,
R,
)
Kc = linalg.solve(R, Bc.T @ Sc).reshape(1, -1)
ct_ctr = LTISystem(-Kc)
Sd = linalg.solve_discrete_are(
Ad,
Bd,
Q,
R,
)
Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad)
dt_ctr = LTISystem(-Kd, dt=dT)
# Equality of discrete-time equivalent and original continuous-time
def continuous_LQR(speed, Q, R, wheelbase=2.995):
A= np.matrix([[0, speed], [0, 0]])
B = np.matrix([[0], [(speed/wheelbase)]])
V = np.matrix(linalg.solve_continuous_are(A, B, Q, R))
K = np.matrix(linalg.inv(R) * (B.T * V))
return K
import scipy
from scipy import linalg
import numpy as np
print('start')
A = np.array([[-1, 1], [-0.5, -0.5]])
B = np.array([[3, 2], [1, 0]])
Q = np.array([[1, 0], [0, 1]])
R = np.array([[2, 0], [0, 2]])
P = linalg.solve_continuous_are(A, B, Q, R)
print(P)
print('Testing:')
test = np.matmul(A.transpose(), P) + np.matmul(P, A) + Q - np.matmul(
P, np.matmul(B, np.matmul(np.linalg.inv(R), np.matmul(B.transpose(), P))))
print(test)
print('done')
#print('BR^ -1 B^t = ', np.matmul(B, np.matmul( np.linalg.inv(R), B.transpose())))
#print('PBR^ -1 B^t P = ', np.matmul(P, np.matmul(B, np.matmul( np.linalg.inv(R), np.matmul(B.transpose(), P)))))
print('optimal control: R^ -1 B^t P = ',
np.matmul(np.linalg.inv(R), np.matmul(B.transpose(), P)))
def solve_ricc(A, E=None, B=None, Q=None, C=None, R=None, G=None,
trans=False, options=None):
"""Find a factor of the solution of a Riccati equation using solve_continuous_are.
Returns factor :math:`Z` such that :math:`Z Z^T` is approximately
the solution :math:`X` of a Riccati equation
.. math::
A^T X E + E^T X A - E^T X B R^{-1} B^T X E + Q = 0.
If E in `None`, it is taken to be the identity matrix.
Q can instead be given as C^T * C. In this case, Q needs to be
`None`, and C not `None`.
B * R^{-1} B^T can instead be given by G. In this case, B and R need
to be `None`, and G not `None`.
If R and G are `None`, then R is taken to be the identity matrix.
If trans is `True`, then the dual Riccati equation is solved
.. math::
A X E^T + E X A^T - E X C^T R^{-1} C X E^T + Q = 0,
where Q can be replaced by B * B^T and C^T * R^{-1} * C by G.
This uses the `scipy.linalg.spla.solve_continuous_are` method.
Generalized Riccati equation is not supported.
It can only solve medium-sized dense problems and assumes access to
the matrix data of all operators.
Parameters
----------
A
The |Operator| A.
B
The |Operator| B or `None`.
E
The |Operator| E or `None`.
Q
The |Operator| Q or `None`.
C
The |Operator| C or `None`.
R
The |Operator| R or `None`.
G
The |Operator| G or `None`.
trans
If the dual equation needs to be solved.
options
The |solver_options| to use (see :func:`ricc_solver_options`).
Returns
-------
Z
Low-rank factor of the Riccati equation solution,
|VectorArray| from `A.source`.
"""
_solve_ricc_check_args(A, E, B, Q, C, R, G, trans)
options = _parse_options(options, lyap_solver_options(), 'scipy', None, False)
assert options['type'] == 'scipy'
if E is not None or G is not None:
raise NotImplementedError
import scipy.linalg as spla
A_mat = to_matrix(A, format='dense')
B_mat = to_matrix(B, format='dense') if B else None
C_mat = to_matrix(C, format='dense') if C else None
Q_mat = to_matrix(Q, format='dense') if Q else None
R_mat = to_matrix(R, format='dense') if R else None
if R is None:
if not trans:
R_mat = np.eye(B.source.dim)
else:
R_mat = np.eye(C.range.dim)
if not trans:
if Q is None:
Q_mat = C_mat.T.dot(C_mat)
X = spla.solve_continuous_are(A_mat, B_mat, Q_mat, R_mat)
else:
if Q is None:
Q_mat = B_mat.dot(B_mat.T)
X = spla.solve_continuous_are(A_mat.T, C_mat.T, Q_mat, R_mat)
Z = chol(X, copy=False)
Z = A.source.from_numpy(np.array(Z).T)
return Z
return optcontrol
### Stuff we care about running
if __name__ == '__main__':
T = round(12 / 0.02)
env = gym.make('CartPoleContinuous-v0')
obs = env.reset()
env.render()
#initial_state = np.array([0,0,.25,0])
#env.state = initial_state
#M,m,l = 1,.1,1
M, m, l = 1, .1, .5
A, B, Q, R = linearized_init(M, m, l, 0, 1, 1, 1, .001)
P = la.solve_continuous_are(A, B.reshape((4, 1)), Q, R)
tol = 1e-2
done = False
for i in range(T):
if la.norm(obs[1:]) < tol:
print("Converged in {} iterations".format(i))
#time.sleep(1)
env.close()
break
# Determine the step size based on our mode
else:
z, u = cart(np.arange(i * .02, T, .02), obs, A, B, Q, R, P)
step = np.array([u[0]])
# Step in designated direction and update the visual
def sdre():
while not rospy.is_shutdown():
now = timer()
global x_r, y_r, detect, x, y, z, roll, pitch, yaw, vel_rover, goal, goal_body, v_x, v_y, v_z, Rot_body_to_inertial, Rot_inertial_to_body
#### Global to Body conversion for the goal
#posn.write('%f;' % float(x))
#posn.write('%f;' % float(z))
#posn.write('%f;' % float(x_r))
#posn.write('%f;' % float(now))
#posn.write('\n')
goal_body[0] = goal[0] - x
goal_body[1] = goal[1] - y
goal_body[2] = goal[2] - z
goal_body = np.dot(Rot_inertial_to_body, goal_body.transpose())
#### Weighting Matrices Q R
Q = np.array(
[[((5 * goal_body[0])**2) / abs(goal_body[2] + 0.0001) + 1, 0, 0,
0, 0, 0],
[
0,
abs(150 * (0.5 + abs(goal_body[2])) * (vel_rover[0] - v_x) /
(0.001 + 0.1 * abs(goal_body[0]))), 0, 0, 0, 0
],
[
0, 0,
((5 * goal_body[1])**2) / abs(goal_body[2] + 0.0001) + 1, 0,
0, 0
],
[
0, 0, 0,
abs(150 * (0.5 + abs(goal_body[2])) * (vel_rover[1] - v_y) /
(0.001 + 0.1 * abs(goal_body[1]))), 0, 0
],
[
0, 0, 0, 0,
1 + (10 * goal_body[2] / sqrt(0.01 + 0.01 *
(goal_body[0]**2) + 0.01 *
(goal_body[1]**2)))**2, 0
], [0, 0, 0, 0, 0, 1 / abs(goal_body[2] + 0.001)]])
R = np.array([
[800, 0, 0] #z - accn
,
[0, 75000, 0] #Pitch
,
[0, 0, 75000]
]) #Roll
#### Calculation for control done in body fixed frame
### d2(e_x)/dt2 = 0-d2(x)/dt2 so all signs inverted
X = np.array([[goal_body[0]], [vel_rover[0] - v_x], [goal_body[1]],
[vel_rover[1] - v_y], [goal_body[2]],
[vel_rover[2] - v_z]])
B = np.array([[0, 0, 0], [0, -9.8, 0], [0, 0, 0], [0, 0, 9.8],
[0, 0, 0], [-1, 0, 0]])
P = la.solve_continuous_are(A, B, Q, R)
u = np.dot(-np.linalg.inv(R), B.transpose())
u = np.dot(u, P)
u = np.dot(u, X)
u0 = float(u[0])
u1 = float(u[1])
u2 = float(u[2])
#### Normalizing the received thrust
u0 = (u0 * 1.5 + 14.7) / 29.4
#### Restrict rotation angles to 10 deg
if u0 > 1:
u0 = 1
if u0 < 0:
u0 = 0
if u1 > 10 * np.pi / 180:
u1 = 10 * np.pi / 180
if u1 < -10 * np.pi / 180:
u1 = -10 * np.pi / 180
if u2 > 10 * np.pi / 180:
u2 = 10 * np.pi / 180
if u2 < -10 * np.pi / 180:
u2 = -10 * np.pi / 180
#### Start descending for small errors
if sqrt(goal_body[0]**2 + goal_body[1]**2) < 0.8 and abs(
goal_body[2]) < 1:
rospy.loginfo("LAND")
u0 = 0.0
u1 = 0.0
u2 = 0.0
#### Convert to quaternions and publish
quater = tf.transformations.quaternion_from_euler(u2, u1, yaw) #0
msg.header = Header()
msg.type_mask = 0
msg.orientation.x = quater[0]
msg.orientation.y = quater[1]
msg.orientation.z = quater[2]
msg.orientation.w = quater[3]
msg.body_rate.x = 0.0
msg.body_rate.y = 0.0
msg.body_rate.z = 0.0
msg.thrust = u0
pub.publish(msg)
rate = rospy.Rate(100)
rate.sleep
def talker():
cmd_publisher = rospy.Publisher('cmd_drive', cmd_drive, queue_size=10)
rospy.Subscriber("/IMU", Odometry, ImuCallback)
rospy.Subscriber("/GPS/fix", NavSatFix, retour_gps)
rospy.init_node('LQR', anonymous=True)
r = rospy.Rate(10) # 10hz
simul_time = rospy.get_param('~simulation_time', '10')
# == Dynamic Model ======================================================
# == Steady State ======================================================
A = get_model_A_matrix()
B = get_model_B_matrix()
Vyss = -(k * vit * (A[(0, 1)] * B[(1, 0)] - A[(1, 1)] * B[(0, 0)] - A[
(0, 1)] * B[(1, 1)] + A[(1, 1)] * B[(0, 1)])) / (A[(0, 0)] * B[
(1, 0)] - A[(1, 0)] * B[(0, 0)] - A[(0, 0)] * B[(1, 1)] +
A[(1, 0)] * B[(0, 1)])
Vpsiss = vit * k
eyss = 0
epsiss = -Vyss / vit
bfss = -(k * vit * (A[(0, 0)] * A[(1, 1)] - A[(0, 1)] * A[
(1, 0)])) / (A[(0, 0)] * B[(1, 0)] - A[(1, 0)] * B[(0, 0)] -
A[(0, 0)] * B[(1, 1)] + A[(1, 0)] * B[(0, 1)])
brss = -bfss
xsss = np.array([[Vyss], [Vpsiss], [eyss], [epsiss]])
usss = np.array([[bfss], [brss]])
# == LQR control (Gains) ======================================================
Q = np.array([[0.0001, 0, 0, 0], [0, 0.0001, 0, 0], [0, 0, 0.0001, 0],
[0, 0, 0, 0.0001]])
R = np.array([[20000, 0], [0, 20000]])
C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
P = solve_continuous_are(A, B, Q, R)
#print(P)
G = np.linalg.inv(R).dot(np.transpose(B)).dot(P)
#print(G)
#np.array([[-0.5,4.3e+13],[-0.5,-4.2e+13]])#
M = pinv(C.dot(pinv(A - B.dot(G))).dot(B))
#print(M)
# == Observer ======================================================
xhat = np.array([[0], [0], [1.5], [0]])
#print(xhat)
Gammax = 0.01 * eye(4, 4)
dt = 0.01 #periode d'échantionnage
Gammaalpha = dt * 0.00001 * eye(4, 4)
Gammabeta = dt**2 * eye(2, 2) * 0.00001
Psiref = 0 * pi / 180
Psipref = 0 * pi / 180
vyref = 0
mat = np.loadtxt('/home/bachir/spido_ws/src/spido_riding/scripts/mat.txt')
coords = curve[:, 1:3]
line = geom.LineString(coords)
t0 = rospy.get_time()
while (rospy.get_time() - t0 <= simul_time):
cmd = cmd_drive()
point = geom.Point(X, Y)
nearest_pt = line.interpolate(line.project(point))
distance, index = spatial.KDTree(coords).query(nearest_pt)
xref = coords[index, 0]
yref = coords[index, 1]
Psiref = curve[index, 3]
ey = -sin(Psiref) * (X - xref) + cos(Psiref) * (Y - yref)
epsi = (Psi - Psiref)
#vy=-Xp*sin(Psi)+Yp*cos(Psi)
vy = xhat[0, 0]
x = np.array([[vy], [Psip], [ey], [epsi]])
yd = np.array([[vyref], [Psipref]])
y = C.dot(x)
#print('u',u)
u = M.dot(yd) - G.dot(x)
xhat, Gammax = kalman(xhat, Gammax, dt * B.dot(u), y, Gammaalpha,
Gammabeta,
eye(4, 4) + dt * A, C)
#print('xhat',xhat)
print(Psi * 180 / pi)
vy1 = -Xp * sin(Psi) + Yp * cos(Psi)
# // vy:vitesse latérale expriméé dans le repère véhicule
print(vy, vy1)
#;//-xpref*sin(psiref)+ypref*cos(psiref); // vy:vitesse latérale expriméé dans le repère véhicule
cmd.linear_speed = vit
cmd.steering_angle_front = u[0, 0]
cmd.steering_angle_rear = u[1, 0]
cmd_publisher.publish(cmd)
r.sleep()
def getRiccatiSolution(self):
S = solve_continuous_are(self.A, self.B, self.Q, self.R, s=self.N)
return S
full[mask] = triu
return full
def full_to_triu(full):
mask = np.triu(np.ones(full.shape, dtype=bool))
return full[mask]
# LISTING_END triuconvert
# LISTING_START riccatiinit
# Get initial value S for matrix Riccati ODE by solving algebraic Riccati equation
A_f, B_f = system_matrices(t_traj[-1], xd_traj[-1], ud_traj[-1], sys_para)
S = scilin.solve_continuous_are(A_f, B_f, Q, R)
# LISTING_END riccatiinit
# LISTING_START riccatiint
Pbar_triu_traj = np.empty(
(n_samples,
int(sys_para.n * (sys_para.n + 1) / 2))) # allocate array for P
Pbar_triu_traj[0, :] = full_to_triu(S) # initialize with Pbar(0) = S
K_traj = np.empty((n_samples, sys_para.m, sys_para.n)) # allocate array for K
# get trajectories for P and K via numerical integration
for i_tau in range(n_samples): # iterate forward in tau direction
i_t = n_samples - 1 - i_tau # index in t vector (counting down from last element)
t_i = t_traj[i_t]
tau_i = t_traj[i_tau]
xd_i = xd_traj[i_t]
inputA.close()
inputB.close()
input_one.close()
input_two.close()
input_three.close()
Q = ((1/.6)**2)*eye(6)
Q[0][0] = ((1.0/0.1)**2)
Q[1][1] = ((1.0/0.1)**2)
Q[2][2] = ((1.0/0.1)**2)
Q[3][3] = ((1.0/0.001)**2)
Q[4][4] = ((1.0/0.001)**2)
Q[5][5] = ((1.0/0.001)**2)
R = eye(3)
R[0][0] = (1/0.000000000000000001)**2
R[1][1] = ((1.0/10.0)**2)
R[2][2] = ((1.0/10.0)**2)
K = []
for a,b in zip(A,B):
S = solve_continuous_are(a,b,Q,R)
K.append(dot(dot(inv(R), b.T), S))
outputK = open('triple_pen_LQR_K_useful.pkl', 'wb')
pickle.dump(K,outputK)
outputK.close()
def LQR(A, B, Q, R):
return la.solve_continuous_are(A, B, Q, R)
def check_case(self, a, b, q, r):
"""Checks if (A'X + XA - XBR^-1B'X+Q=0) is true"""
x = solve_continuous_are(a, b, q, r)
assert_array_almost_equal(
a.getH()*x + x*a - x*b*inv(r)*b.getH()*x + q, 0.0)
def solve_lqr(A, B, Q, R):
P = solve_continuous_are(A, B, Q, R)
K = np.dot(np.dot(np.linalg.inv(R), B.T), P)
return K
def run_cl_simulation(rom, name, Re=110, level=2, palpha=1e-3, control='bc'):
"""Run the closed-loop simulation with reduced LQG controller."""
# define strings, directories and paths for data storage
setup_str = 'lvl_' + str(level) + ('_' + control if control is not None else '') \
+ '_re_' + str(Re) + ('_palpha_' + str(palpha) if control == 'bc' else '')
data_path = '../data/' + 'lvl_' + str(level) + ('_' + control if control
is not None else '')
setup_path = data_path + '/re_' + str(Re) + ('_palpha_' + str(palpha)
if control == 'bc' else '')
simulation_path = setup_str + '/' + name + '_simulation'
if not os.path.exists(simulation_path):
os.makedirs(simulation_path)
with open(simulation_path + '/rom_' + str(rom.order) + '.csv',
'w') as file:
file.write('t, yerr \n')
# define first order model and matrices for simulation
fom = load_fom(Re=110, level=2, palpha=1e-3, control='bc')
mats = spio.loadmat(data_path + '/mats')
M = mats['M']
J = mats['J']
hmat = mats['H']
fv = mats['fv'] + 1. / Re * mats['fv_diff'] + mats['fv_conv']
fp = mats['fp'] + mats['fp_div']
vcmat = mats['Cv']
NV, NP = fv.shape[0], fp.shape[0]
if control == 'bc':
A = 1. / Re * mats['A'] + mats['L1'] + mats[
'L2'] + 1. / palpha * mats['Arob']
B = 1. / palpha * mats['Brob']
else:
A = 1. / Re * mats['A'] + mats['L1'] + mats['L2']
B = mats['B']
# restrict to less dofs in the input
NU = B.shape[1]
B = B[:, [0, NU // 2]]
# compute steady-state solution and linearized convection
if not os.path.isfile(setup_path + '/ss_nse_sol'):
ss_nse_v, _ = solve_steadystate_nse(mats, Re, control, palpha=palpha)
conv_mat = linearized_convection(mats['H'], ss_nse_v)
spio.savemat(setup_path + '/ss_nse_sol', {
'ss_nse_v': ss_nse_v,
'conv_mat': conv_mat
})
else:
ss_nse_sol = spio.loadmat(setup_path + '/ss_nse_sol')
ss_nse_v, conv_mat = ss_nse_sol['ss_nse_v'], ss_nse_sol['conv_mat']
# Define parameters for time stepping
t0 = 0.
tE = 8.
Nts = 2**12
DT = (tE - t0) / Nts
trange = np.linspace(t0, tE, Nts + 1)
# Define functions that represent the system inputs
if control is None:
def bbcu(ko_state):
return np.zeros((NV, 1))
def update_ko_state(ko_state, Cv, DT):
return ko_state
else:
Arom = to_matrix(rom.A, format='dense') # .real?
Brom = to_matrix(rom.B, format='dense')
Crom = to_matrix(rom.C, format='dense')
XCARE = spla.solve_continuous_are(Arom,
Brom,
Crom.T @ Crom,
np.eye(Brom.shape[1]),
balanced=False)
XFARE = spla.solve_continuous_are(Arom.T,
Crom.T,
Brom @ Brom.T,
np.eye(Crom.shape[0]),
balanced=False)
# define control based on Kalman observer state
def bbcu(ko_state):
uvec = -Brom.T @ XCARE @ ko_state
return B @ uvec
ko1_mat = Arom - XFARE @ Crom.T @ Crom - Brom @ Brom.T @ XCARE
ko2_mat = XFARE @ Crom.T
lu_piv = spla.lu_factor(np.eye(rom.order) - DT * ko1_mat)
Css = vcmat @ ss_nse_v
# function that determines the next state of the Kalman observer via implicit euler step
def update_ko_state(ko_state, Cv, DT):
return spla.lu_solve(lu_piv, ko_state + DT * ko2_mat @ (Cv - Css))
# introduce small perturbation to steady-state solution as initial value
pert = fom.A.source.project_onto_subspace(fom.A.operator.source.ones(),
trans=True).to_numpy().T
old_v = ss_nse_v + 1e-3 * pert
# initialize state for observer
ko_state = np.zeros((rom.order, 1))
sysmat = sps.vstack([
sps.hstack([M + DT * A, -J.T]),
sps.hstack([J, sps.csc_matrix((NP, NP))])
]).tocsc()
sysmati = spsla.factorized(sysmat)
try:
for k, t in enumerate(trange):
crhsv = M * old_v + DT * (fv - eva_quadterm(hmat, old_v) +
bbcu(ko_state))
crhs = np.vstack([crhsv, fp])
vp_new = np.atleast_2d(sysmati(crhs.flatten())).T
old_v = vp_new[:NV]
Cv = vcmat @ old_v
ko_state = update_ko_state(ko_state, Cv, DT)
print(k, '/', Nts)
print(spla.norm(Cv - Css, 2))
with open(simulation_path + '/rom_' + str(rom.order) + '.csv',
'a') as file:
file.write(str(t) + ',' + str(spla.norm(Cv - Css, 2)) + '\n')
except:
with open('simulation_error_log.txt', 'a') as file:
file.write(name + '_' + str(rom.order) + '\n')
def lqr_kp(self):
A, B, _, _ = self.get_state_matrices()
P = solve_continuous_are(A, B, self.Q, self.R)
K = self.R.I * B.T * P
return K, P
def test_solve_continuous_are():
mat6 = _load_data('carex_6_data.npz')
mat15 = _load_data('carex_15_data.npz')
mat18 = _load_data('carex_18_data.npz')
mat19 = _load_data('carex_19_data.npz')
mat20 = _load_data('carex_20_data.npz')
cases = [
# Carex examples taken from (with default parameters):
# [1] P.BENNER, A.J. LAUB, V. MEHRMANN: 'A Collection of Benchmark
# Examples for the Numerical Solution of Algebraic Riccati
# Equations II: Continuous-Time Case', Tech. Report SPC 95_23,
# Fak. f. Mathematik, TU Chemnitz-Zwickau (Germany), 1995.
#
# The format of the data is (a, b, q, r, knownfailure), where
# knownfailure is None if the test passes or a string
# indicating the reason for failure.
#
# Test Case 0: carex #1
(np.diag([1.], 1), np.array([[0], [1]]), block_diag(1., 2.), 1, None),
# Test Case 1: carex #2
(np.array([[4, 3], [-4.5, -3.5]]), np.array([[1], [-1]]),
np.array([[9, 6], [6, 4.]]), 1, None),
# Test Case 2: carex #3
(np.array([[0, 1, 0, 0], [0, -1.89, 0.39, -5.53],
[0, -0.034, -2.98, 2.43], [0.034, -0.0011, -0.99, -0.21]]),
np.array([[0, 0], [0.36, -1.6], [-0.95, -0.032], [0.03, 0]]),
np.array([[2.313, 2.727, 0.688, 0.023], [2.727, 4.271, 1.148, 0.323],
[0.688, 1.148, 0.313, 0.102], [0.023, 0.323, 0.102,
0.083]]), np.eye(2), None),
# Test Case 3: carex #4
(np.array([[-0.991, 0.529, 0, 0, 0, 0, 0, 0],
[0.522, -1.051, 0.596, 0, 0, 0, 0, 0],
[0, 0.522, -1.118, 0.596, 0, 0, 0, 0],
[0, 0, 0.522, -1.548, 0.718, 0, 0, 0],
[0, 0, 0, 0.922, -1.64, 0.799, 0, 0],
[0, 0, 0, 0, 0.922, -1.721, 0.901, 0],
[0, 0, 0, 0, 0, 0.922, -1.823, 1.021],
[0, 0, 0, 0, 0, 0, 0.922, -1.943]]),
np.array([[3.84, 4.00, 37.60, 3.08, 2.36, 2.88, 3.08, 3.00],
[-2.88, -3.04, -2.80, -2.32, -3.32, -3.82, -4.12, -3.96]]).T
* 0.001,
np.array([[1.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.1],
[0.0, 1.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0, 0.0, 0.5, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
[0.5, 0.1, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.5, 0.0, 0.0, 0.1, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.1]]), np.eye(2), None),
# Test Case 4: carex #5
(np.array(
[[-4.019, 5.120, 0., 0., -2.082, 0., 0., 0., 0.870],
[-0.346, 0.986, 0., 0., -2.340, 0., 0.,
0., 0.970],
[-7.909, 15.407, -4.069, 0., -6.450, 0., 0., 0., 2.680],
[-21.816, 35.606, -0.339, -3.870, -17.800, 0., 0., 0., 7.390],
[-60.196, 98.188, -7.907, 0.340, -53.008, 0., 0., 0., 20.400],
[0, 0, 0, 0, 94.000, -147.200, 0., 53.200, 0.],
[0, 0, 0, 0, 0, 94.000, -147.200, 0, 0],
[0, 0, 0, 0, 0, 12.800, 0.000, -31.600, 0],
[0, 0, 0, 0, 12.800, 0.000, 0.000, 18.800, -31.600]]),
np.array([[0.010, -0.011, -0.151], [0.003, -0.021, 0.000],
[0.009, -0.059, 0.000], [0.024, -0.162, 0.000],
[0.068, -0.445, 0.000], [0.000, 0.000, 0.000],
[0.000, 0.000, 0.000], [0.000, 0.000, 0.000],
[0.000, 0.000, 0.000]]), np.eye(9), np.eye(3), None),
# Test Case 5: carex #6
(mat6['A'], mat6['B'], mat6['Q'], mat6['R'], None),
# Test Case 6: carex #7
(np.array([[1, 0], [0, -2.]]), np.array([[1e-6], [0]]), np.ones(
(2, 2)), 1., 'Bad residual accuracy'),
# Test Case 7: carex #8
(block_diag(-0.1, -0.02), np.array([[0.100, 0.000], [0.001, 0.010]]),
np.array([[100, 1000], [1000, 10000]]), np.ones(
(2, 2)) + block_diag(1e-6, 0), None),
# Test Case 8: carex #9
(np.array([[0, 1e6], [0, 0]]), np.array([[0],
[1.]]), np.eye(2), 1., None),
# Test Case 9: carex #10
(np.array([[1.0000001, 1],
[1., 1.0000001]]), np.eye(2), np.eye(2), np.eye(2), None),
# Test Case 10: carex #11
(np.array([[3, 1.], [4, 2]]), np.array([[1], [1]]),
np.array([[-11, -5], [-5, -2.]]), 1., None),
# Test Case 11: carex #12
(np.array([[7000000., 2000000., -0.], [2000000., 6000000., -2000000.],
[0., -2000000., 5000000.]]) / 3, np.eye(3),
np.array([[1., -2., -2.], [-2., 1., -2.], [-2., -2., 1.]]).dot(
np.diag([1e-6, 1, 1e6])).dot(
np.array([[1., -2., -2.], [-2., 1., -2.], [-2., -2., 1.]])) /
9, np.eye(3) * 1e6, 'Bad Residual Accuracy'),
# Test Case 12: carex #13
(np.array([[0, 0.4, 0, 0], [0, 0, 0.345, 0],
[0, -0.524e6, -0.465e6, 0.262e6], [0, 0, 0, -1e6]]),
np.array([[0, 0, 0, 1e6]]).T, np.diag([1, 0, 1, 0]), 1., None),
# Test Case 13: carex #14
(np.array([[-1e-6, 1, 0, 0], [-1, -1e-6, 0, 0], [0, 0, 1e-6, 1],
[0, 0, -1, 1e-6]]), np.ones((4, 1)), np.ones(
(4, 4)), 1., None),
# Test Case 14: carex #15
(mat15['A'], mat15['B'], mat15['Q'], mat15['R'], None),
# Test Case 15: carex #16
(np.eye(64, 64, k=-1) + np.eye(64, 64) * (-2.) +
np.rot90(block_diag(1, np.zeros((62, 62)), 1)) + np.eye(64, 64, k=1),
np.eye(64), np.eye(64), np.eye(64), None),
# Test Case 16: carex #17
(np.diag(np.ones((20, )), 1), np.flipud(np.eye(21, 1)),
np.eye(21, 1) * np.eye(21, 1).T, 1, 'Bad Residual Accuracy'),
# Test Case 17: carex #18
(mat18['A'], mat18['B'], mat18['Q'], mat18['R'], None),
# Test Case 18: carex #19
(mat19['A'], mat19['B'], mat19['Q'], mat19['R'],
'Bad Residual Accuracy'),
# Test Case 19: carex #20
(mat20['A'], mat20['B'], mat20['Q'], mat20['R'],
'Bad Residual Accuracy')
]
# Makes the minimum precision requirements customized to the test.
# Here numbers represent the number of decimals that agrees with zero
# matrix when the solution x is plugged in to the equation.
#
# res = array([[8e-3,1e-16],[1e-16,1e-20]]) --> min_decimal[k] = 2
#
# If the test is failing use "None" for that entry.
#
min_decimal = (14, 12, 13, 14, 11, 6, None, 5, 7, 14, 14, None, 9, 14, 13,
14, None, 12, None, None)
def _test_factory(case, dec):
"""Checks if 0 = XA + A'X - XB(R)^{-1} B'X + Q is true"""
a, b, q, r, knownfailure = case
if knownfailure:
pytest.xfail(reason=knownfailure)
x = solve_continuous_are(a, b, q, r)
res = x.dot(a) + a.conj().T.dot(x) + q
out_fact = x.dot(b)
res -= out_fact.dot(solve(np.atleast_2d(r), out_fact.conj().T))
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def solve_ricc_lrcf(A, E, B, C, R=None, S=None, trans=False, options=None):
"""Compute an approximate low-rank solution of a Riccati equation.
See :func:`pymor.algorithms.riccati.solve_ricc_lrcf` for a general
description.
This function uses `scipy.linalg.solve_continuous_are`, which
is a dense solver.
Therefore, we assume all |Operators| and |VectorArrays| can be
converted to |NumPy arrays| using
:func:`~pymor.algorithms.to_matrix.to_matrix` and
:func:`~pymor.vectorarrays.interfaces.VectorArrayInterface.to_numpy`.
Parameters
----------
A
The |Operator| A.
E
The |Operator| E or `None`.
B
The operator B as a |VectorArray| from `A.source`.
C
The operator C as a |VectorArray| from `A.source`.
R
The operator R as a 2D |NumPy array| or `None`.
S
The operator S as a |VectorArray| from `A.source` or `None`.
trans
Whether the first |Operator| in the Riccati equation is
transposed.
options
The solver options to use (see :func:`ricc_lrcf_solver_options`).
Returns
-------
Z
Low-rank Cholesky factor of the Riccati equation solution,
|VectorArray| from `A.source`.
"""
_solve_ricc_check_args(A, E, B, C, R, S, trans)
options = _parse_options(options, ricc_lrcf_solver_options(), 'scipy', None, False)
if options['type'] != 'scipy':
raise ValueError(f"Unexpected Riccati equation solver ({options['type']}).")
A_source = A.source
A = to_matrix(A, format='dense')
E = to_matrix(E, format='dense') if E else None
B = B.to_numpy().T
C = C.to_numpy()
S = S.to_numpy().T if S else None
if R is None:
R = np.eye(C.shape[0] if not trans else B.shape[1])
if not trans:
if E is not None:
E = E.T
X = solve_continuous_are(A.T, C.T, B.dot(B.T), R, E, S)
else:
X = solve_continuous_are(A, B, C.T.dot(C), R, E, S)
return A_source.from_numpy(_chol(X).T)
