def dare(A, B, Q, R, S=None, E=None, stabilizing=True):
""" (X,L,G) = dare(A,B,Q,R) solves the discrete-time algebraic Riccati
equation
:math:`A^T X A - X - A^T X B (B^T X B + R)^{-1} B^T X A + Q = 0`
where A and Q are square matrices of the same dimension. Further, Q
is a symmetric matrix. The function returns the solution X, the gain
matrix G = (B^T X B + R)^-1 B^T X A and the closed loop eigenvalues L,
i.e., the eigenvalues of A - B G.
(X,L,G) = dare(A,B,Q,R,S,E) solves the generalized discrete-time algebraic
Riccati equation
:math:`A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^{-1} (B^T X A + S^T) + Q = 0`
where A, Q and E are square matrices of the same dimension. Further, Q and
R are symmetric matrices. The function returns the solution X, the gain
matrix :math:`G = (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop
eigenvalues L, i.e., the eigenvalues of A - B G , E.
"""
if S is not None or E is not None or not stabilizing:
return dare_old(A, B, Q, R, S, E, stabilizing)
else:
Rmat = asmatrix(R)
Qmat = asmatrix(Q)
X = solve_discrete_are(A, B, Qmat, Rmat)
G = solve(B.T.dot(X).dot(B) + Rmat, B.T.dot(X).dot(A))
L = eigvals(A - B.dot(G))
return X, L, G
def check_case(self, a, b, q, r):
"""Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true"""
x = solve_discrete_are(a, b, q, r)
assert_array_almost_equal(
a.getH() * x * a - (a.getH() * x * b) * inv(r + b.getH() * x * b) * (b.getH() * x * a) + q - x, 0.0
)
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, knownfailure = case
if knownfailure:
pytest.xfail(reason=knownfailure)
x = solve_discrete_are(a, b, q, r)
res = a.conj().T.dot(x.dot(a)) - x + q
res -= a.conj().T.dot(x.dot(b)).dot(
solve(r+b.conj().T.dot(x.dot(b)), b.conj().T).dot(x.dot(a))
)
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def S_riccatti(self): """ Solve for cost-to-go matrix using (discrete-time) Riccatti equation. """ A = self.A B = self.B Q = self.Q R = self.R S = solve_discrete_are(A, B, Q, R) S = torch.tensor(S).float() return S
def dkalman(A, C, W, V):
"""Solve the infinite-horizon discrete-time Kalman observer.
x[k+1] = A x[k] + B u[k] + w[y]
y[k] = C x[k] + v[t]
"""
#first, try to solve the ricatti equation
S = la.solve_discrete_are(A.T, C.T, W, V)
#compute the Kalman gain
L = la.solve(C.dot(S).dot(C.T) + V, C.dot(S).dot(A.T)).T
eigVals, eigVecs = la.eig(A - L.dot(C))
return L, S, eigVals
def dlqr(A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = la.solve_discrete_are(A, B, Q, R)
#compute the LQR gain
K = la.pinv(
np.dot((np.dot(np.dot(B.T, X), B) + R), np.dot(np.dot(B.T, X), A)).T)
eigVals, eigVecs = la.eig(A - np.dot(B, K))
return K, eigVals
def _dlqr(A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#first, try to solve the ricatti equation
X = la.solve_discrete_are(A, B, Q, R)
#compute the LQR gain
K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
eigVals, eigVecs = la.eig(A - B @ K)
return K, X, eigVals
def inf_cost(A, B, C, Q, Ru, Rv, K, L):
d, p = B.shape
a, b = C.shape
K = K.reshape((p, d))
L = L.reshape((b, a))
R = np.linalg.diag((Ru, Rv))
B_t = np.hstack((B, C))
cl_map = A + np.matmul(B, K) + np.matmul(C, L)
if np.amax(np.abs(LA.eigvals(cl_map))) < (1.0 - 1.0e-6):
cost = np.trace(LA.solve_discrete_are(A, B_t, Q, R))
else:
#cost = float("inf")
cost = -20
return cost
def test_solve_generalized_discrete_are():
mat20170120 = np.load(
os.path.join(os.path.abspath(os.path.dirname(__file__)), 'data',
'gendare_20170120_data.npz'))
cases = [
# Two random examples differ by s term
# in the absence of any literature for demanding examples.
(np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01],
[4.617139e-02, 6.948286e-01, 3.444608e-02],
[9.713178e-02, 3.170995e-01, 4.387444e-01]]),
np.array([[3.815585e-01, 1.868726e-01], [7.655168e-01, 4.897644e-01],
[7.951999e-01, 4.455862e-01]]), np.eye(3), np.eye(2),
np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01],
[7.093648e-01, 6.797027e-01, 1.189977e-01],
[7.546867e-01, 6.550980e-01,
4.983641e-01]]), np.zeros((3, 2)), None),
(np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01],
[4.617139e-02, 6.948286e-01, 3.444608e-02],
[9.713178e-02, 3.170995e-01, 4.387444e-01]]),
np.array([[3.815585e-01, 1.868726e-01], [7.655168e-01, 4.897644e-01],
[7.951999e-01, 4.455862e-01]]), np.eye(3), np.eye(2),
np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01],
[7.093648e-01, 6.797027e-01, 1.189977e-01],
[7.546867e-01, 6.550980e-01, 4.983641e-01]]), np.ones(
(3, 2)), None),
# user-reported (under PR-6616) 20-Jan-2017
# tests against the case where E is None but S is provided
(mat20170120['A'], mat20170120['B'], mat20170120['Q'],
mat20170120['R'], None, mat20170120['S'], None),
]
min_decimal = (11, 11, 16)
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:
raise KnownFailureTest(knownfailure)
x = solve_discrete_are(a, b, q, r, e, s)
if e is None:
e = np.eye(a.shape[0])
if s is None:
s = np.zeros_like(b)
res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q
res -= (a.conj().T.dot(x.dot(b)) + s).dot(
solve(r + b.conj().T.dot(x.dot(b)),
(b.conj().T.dot(x.dot(a)) + s.conj().T)))
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
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_gain(A,B,Q,R):
'''
Arguments:
State transition matrices (A,B)
LQR Costs (Q,R)
Outputs:
K: optimal infinite-horizon LQR gain matrix given
'''
# solve DARE:
M=LA.solve_discrete_are(A,B,Q,R)
# K=(B'MB + R)^(-1)*(B'MA)
return np.dot(LA.inv(np.dot(np.dot(B.T,M),B)+R),(np.dot(np.dot(B.T,M),A)))
def dlqr(A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = la.solve_discrete_are(A, B, Q, R)
#compute the LQR gain
K = la.solve(B.T.dot(X).dot(B) + R, B.T.dot(X).dot(A))
eigVals, eigVecs = la.eig(A - B.dot(K))
return K, X, eigVals
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:
raise KnownFailureTest(knownfailure)
x = solve_discrete_are(a, b, q, r, e, s)
res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q
res -= (a.conj().T.dot(x.dot(b)) + s).dot(
solve(r+b.conj().T.dot(x.dot(b)),
(b.conj().T.dot(x.dot(a)) + s.conj().T)
)
)
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def lqr_gain(self, given_q, given_r):
'''
lqr gain for the system
:param given_q:
:param given_r:
:return: the kernel of the Lyapunov function P and the gain K
'''
try:
P = LA.solve_discrete_are(self.A, self.B, given_q, given_r)
K = -LA.inv(self.B.T @ P @ self.B + given_r) @ self.B.T @ P @ self.A
except:
P = np.zeros((self.n, self.n))
K = np.zeros((self.m, self.n))
return P, K
def __init__(self, A, B, Q=None, R=None):
# https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
A = np.array(A)
B = np.array(B)
# quadratic error matrices
if Q is None:
Q = np.eye(A.shape[0])
if R is None:
R = np.zeros((B.shape[1], B.shape[1]))
P = solve_discrete_are(A, B, Q, R)
self.policy = -np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)
def dlqr(A: np.array, B: np.array, Q: np.array, R: np.array, with_eigs=False) \
-> np.array:
P = lin.solve_discrete_are(A, B, Q, R)
if np.size(R) == 1:
K = (np.transpose(B).dot(P)) / R
else:
K = nla.inv(R).dot((np.transpose(B).dot(P)))
eig_vals, eig_vecs = nla.eig(A - B.dot(K))
if with_eigs:
return K, P, eig_vals, eig_vecs
else:
return K, P
def dlqr(A, B, Q, R):
'''
Solves for the optimal infinite-horizon, discrete-time LQR controller
given linear system (A,B) and cost function parameterized by (Q,R)
'''
S = sl.solve_discrete_are(A, B, Q, R)
F = np.matmul(sl.inv(np.matmul(np.matmul(B.T, S), B) + R),
(np.matmul(np.matmul(B.T, S), A)))
eigVals, eigVecs = sl.eig(A - np.matmul(B, F))
return F, S, eigVals
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 compute_gains(self, position_weight=1.0, pitch_weight=1.0, control_weight=1.0, dt=0.01):
Q = np.diag([position_weight, 0, pitch_weight, 0.0])
R = np.diag([control_weight])
A_de, B_de, C_de, D_de, dt = signal.cont2discrete((self.A, self.B, self.C, self.D), dt)
# solve Discrete Ricatti Equation
P = linalg.solve_discrete_are(A_de, B_de, Q, R)
# compute optimum controller gains
Bp = np.linalg.multi_dot([B_de.T, P, B_de])
Bp = np.linalg.inv((R+Bp))
gains = np.linalg.multi_dot([Bp, B_de.T, P, A_de])
return gains
def test_solve_generalized_discrete_are():
cases = [
# Two random examples differ by s term
# in the absence of any literature for demanding examples.
(np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01],
[4.617139e-02, 6.948286e-01, 3.444608e-02],
[9.713178e-02, 3.170995e-01, 4.387444e-01]]),
np.array([[3.815585e-01, 1.868726e-01],
[7.655168e-01, 4.897644e-01],
[7.951999e-01, 4.455862e-01]]),
np.eye(3),
np.eye(2),
np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01],
[7.093648e-01, 6.797027e-01, 1.189977e-01],
[7.546867e-01, 6.550980e-01, 4.983641e-01]]),
np.zeros((3, 2)),
None),
(np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01],
[4.617139e-02, 6.948286e-01, 3.444608e-02],
[9.713178e-02, 3.170995e-01, 4.387444e-01]]),
np.array([[3.815585e-01, 1.868726e-01],
[7.655168e-01, 4.897644e-01],
[7.951999e-01, 4.455862e-01]]),
np.eye(3),
np.eye(2),
np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01],
[7.093648e-01, 6.797027e-01, 1.189977e-01],
[7.546867e-01, 6.550980e-01, 4.983641e-01]]),
np.ones((3, 2)),
None)
]
min_decimal = (11, 11)
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:
raise KnownFailureTest(knownfailure)
x = solve_discrete_are(a, b, q, r, e, s)
res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q
res -= (a.conj().T.dot(x.dot(b)) + s).dot(
solve(r + b.conj().T.dot(x.dot(b)),
(b.conj().T.dot(x.dot(a)) + s.conj().T)
)
)
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def LQR_control():
# Cost matrices for LQR
Q = np.diag(np.array([1, 1, 1, 1])) # state_dimension = 4
R = np.eye(1) # control_dimension = 1
A, B = computeAB(np.array([0.0, 0.0, 0.0, 0.0]), np.array([0.0]))
# Use if discrete forward dynamics is used
X = sp_linalg.solve_discrete_are(A, B, Q, R)
K = np.dot(np.linalg.pinv(R + np.dot(B.T, np.dot(X, B))),
np.dot(B.T, np.dot(X, A)))
# Use for continuous version of LQR
# X = sp_linalg.solve_continuous_are(A, B, Q, R)
# K = np.dot(np.linalg.pinv(R), np.dot(B.T, X))
return np.squeeze(K, 0)
def dlqr(A,B,Q,R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = np.matrix(la.solve_discrete_are(A, B, Q, R))
#compute the LQR gain
K = np.matrix(la.inv(B.T*X*B+R)*(B.T*X*A))
eigVals, eigVecs = la.eig(A-B*K)
return K, X, eigVals
def dlqr(a, b, q, r):
""" Get the feedback controls from linearized system at the current time step
for a discrete time system Ax+Bu
find the infinite horizon optimal feedback controller
to steer the system to the origin
with
u = -K*x
"""
x = np.matrix(sLa.solve_discrete_are(a, b, q, r))
k = np.matrix(sLa.inv(b.T * x * b + r) * (b.T * x * a))
eigVals, eigVecs = sLa.eig(a - b * k)
return np.asarray(k), np.asarray(x), eigVals
def __init__(self, Y, A0, A1, U0, U1, Q, Phi, initV='unconditional', X0 = None, V0 = None, statevar_names = None):
self.Y, self.A0, self.A1, self.U0, self.U1, self.Q, self.Phi, self.initV, self.X0, self.V0, self.statevar_names = Y, A0, A1, U0, U1, \
Q, Phi, initV, X0, V0, statevar_names
n = self.U0.size
if self.X0 is None: # initialize X0 at unconditional mean
self.X0 = np.mat(np.linalg.inv(np.identity(n) - self.U1)*self.U0)
if self.V0 is None:
if initV == 'steady_state': # solve discrete Ricatti equation
self.V0 = np.mat(solve_discrete_are(np.array(self.U1.T), np.array(self.A1.T), np.array(self.Q), np.array(self.Phi.T)))
elif initV == 'unconditional': # solve discrete Lyapunov equation
self.V0 = np.mat(solve_discrete_lyapunov(np.array(self.U1.T), np.array(self.Q)))
elif initV == 'identity':
self.V0 = np.mat(np.identity(n))
if statevar_names is None:
self.statevar_names = np.arange(len(self.X0))
Kalman.kalmanCount += 1
def compute_steady_state_P(self):
'''
Computes the process to get P (covariance matrix on state) in steady
state as the solution of a discrete Ricatti equation.
Returns
-------
ss_P: matrix
Covariance matrix on state when the system is on steady state.
Notes
-----
This function should be used if the system is equiped with some kind of
feedback linearization.
'''
ss_P = solve_discrete_are(self.kf.F.T, self.kf.H.T, self.kf.Q, self.kf.R)
return ss_P
def GetLQRController(A, B, C, ro):
"""Solve the discrete time lqr controller.
x[k+1] = A*x[k] + B*u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#ref Bertsekas, p.151
Q = numpy.dot(numpy.transpose(C), C)
R = ro * numpy.eye(len(B[0]))
#first, try to solve the ricatti equation
X = linalg.solve_discrete_are(A, B, Q, R)
#compute the LQR gain
G = linalg.inv(numpy.transpose(B) @ X @ B +
R) @ (numpy.transpose(B) @ X @ A)
return G
def calc_NN_LQR(self, Kw, Cw, Bw, Q=None, R=None):
N = len(Kw)
if Q == None:
Q = np.zeros((N + 1, N + 1))
Q[-1, -1] = 100
if R == None: R = np.array([[1]])
#Linearised model
A = np.concatenate([Kw, -Cw * self.dt * self.xmax[1]], axis=0)
A = np.concatenate([A, np.zeros((N + 1, 1))], axis=1)
A[-1, -1] = 1
B = np.concatenate([Bw, np.zeros((1, 1))], axis=0)
S = la.solve_discrete_are(A, B, Q, R)
self.Knn = np.matmul(la.inv(R + np.matmul(np.matmul(B.T, S), B)),
np.matmul(np.matmul(B.T, S), A))
return self.Knn
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_discrete_are(a, b, q, r, e, s)
if e is None:
e = np.eye(a.shape[0])
if s is None:
s = np.zeros_like(b)
res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q
res -= (a.conj().T.dot(x.dot(b)) + s).dot(
solve(r+b.conj().T.dot(x.dot(b)),
(b.conj().T.dot(x.dot(a)) + s.conj().T)
)
)
assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def LQR_discrete_gain(dt):
A = np.array([
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, g, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, -g, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
])
A = np.eye(12) + A*dt
B = np.array([
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[1/mass, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 1/Ix, 0, 0],
[0, 0, 1/Iy, 0],
[0, 0, 0, 1/Iz],
])
B = B*dt
# Q = np.eye(12)*1.0
Q = np.diag([0.4, 0.4, 0.4, 0.1, 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1])*4
R = np.diag([0.1,0.2,0.2,0.2])*0.001
# Q = np.diag([1, 1.0, 3.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
# R = np.eye(4)*0.001
P = solve_discrete_are(A, B, Q, R)
K = np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)
return K
def control(self, arm):
x_desired = self.x - self.target
self.arm, x = self.copy_arm(arm)
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
A, B = self.finite_diff_method(x, self.u)
P = linalg.solve_discrete_are(A, B, self.Q, self.R)
K = -np.dot(np.linalg.pinv(self.R + np.dot(B.T, np.dot(P, B))),
np.dot(B.T, np.dot(P, A)))
J = self.arm.gen_jacEE()
x = np.hstack([np.dot(J.T, x_desired)], arm.dq)
self.u = np.dot(K, x)
return self.u
def kalGain(f):
"""
L = kalGain(f) solves the Discrete-time Algebraic Riccatti
Equation (DARE) for X:
X = AXA' - (AXC') * (R+CXC')^-1 * (CXA') + Q
Then computes and returns the steady-state Kalman gain, L:
L = XC' * (CXC'+R)^-1
f : agent
Q : covariance of state error
R : covariance of measurement error
"""
Q = np.matrix( f.stateCov )
R = np.matrix( f.sensorCov1 )
X = linalg.solve_discrete_are( f.A.T, f.C.T, Q, R )
L = X * f.C.T * linalg.inv( f.C * X * f.C.T + R )
return L
def dlqr(A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
"""
#ref Bertsekas, p.151
#first, try to solve the ricatti equation
X = np.array(solve_discrete_are(A, B, Q, R))
#compute the LQR gain
K = np.array(inv(B.T @ X @ B + R) * (B.T @ X @ A))
eigVals, _ = eig(A - B @ K)
return K, X, eigVals
def __init__(self):
self.dt = 0.1 # sampling time (should prob use dw timestamp)
self.xhat = np.transpose(np.array([[4., 0., 2., 0.]])) # state est
self.yt = np.transpose(np.array([[0., 0., 0., 0.]])) # residual
self.u = np.transpose(np.array([[0., 0.]])) # input
self.z = np.transpose(np.array([[0., 0.]])) # dw measurement
# Statespace matrices
self.A = np.array([[1., self.dt, 0., 0.],
[0., 1., 0., 0],
[0., 0., 1., self.dt],
[0., 0., 0., 1.]])
self.B = np.array([[0, 0],
[self.dt, 0],
[0, 0],
[0, self.dt]])
self.AT = np.transpose(self.A)
self.C = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0]])
self.CT = np.transpose(self.C)
# Kalman matrices
self.Q = np.array([[10., 0., 1., 0.],
[0., 0.1, 0., 0],
[1., 0., 1., 0.],
[0., 0., 0., 0.1]])
self.R = np.array([[10000., 100.],
[100., 10000.]])
self.S = np.array([[0., 0.],
[0., 0.]])
# Riccatti
self.P = linalg.solve_discrete_are(a=np.transpose(self.A),
b=np.transpose(self.C), q=self.Q, r=self.R)
def control_mpc(self, x0):
# LQR-MPC parameters
if not hasattr(self, "Q"):
self.Q = np.eye(2)
if not hasattr(self, "R"):
self.R = 1
if not hasattr(self, "Pinf"):
self.Pinf = solve_discrete_are(self.At, self.bt, self.Q, self.R)
return control_mpc(
x0,
self.At,
self.bt,
self.ct,
self.Q,
self.R,
self.Pinf,
self.u_limits[:, 0],
self.u_limits[:, 1],
n_mpc=10,
debug=False,
)
def control_systems(request):
ct_sys, ref = request.param
Ac, Bc, Cc = ct_sys.data
Dc = np.zeros((Cc.shape[0], 1))
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.reshape(-1, 1),
Q,
R,
)
Kc = linalg.solve(R, Bc.T @ Sc).reshape(1, -1)
ct_ctr = LTISystem(Kc)
evals = np.sort(
np.abs(linalg.eig(Ac, left=False, right=False, check_finite=False)))
dT = 1 / (2 * evals[-1])
Tsim = (8 / np.min(evals[~np.isclose(evals, 0)])
if np.sum(np.isclose(evals[np.nonzero(evals)], 0)) > 0 else 8)
dt_data = signal.cont2discrete((Ac, Bc.reshape(-1, 1), Cc, Dc), dT)
Ad, Bd, Cd, Dd = dt_data[:-1]
Sd = linalg.solve_discrete_are(
Ad,
Bd.reshape(-1, 1),
Q,
R,
)
Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad)
dt_sys = LTISystem(Ad, Bd, dt=dT)
dt_sys.initial_condition = ct_sys.initial_condition
dt_ctr = LTISystem(Kd, dt=dT)
yield ct_sys, ct_ctr, dt_sys, dt_ctr, ref, Tsim
def defVfin(x, xs, **Tcost):
"""
SUMMARY:
It constructs the terminal cost for the dynamic optimisation objective function
SYNTAX:
assignment = defVfin(x, **Tcost):
ARGUMENTS:
+ x - State symbolic variable
+ Tcost - Terminal weigth specified by the user/ Matrices to calculate Riccati equation
OUTPUTS:
+ Vfin - terminal cost for the dynamic optimisation objective function
"""
if Tcost == {}:
vfin = 0.0
else:
for key in Tcost:
if key == 'A': # Linear system & QP problem
A = Tcost['A']
B = Tcost['B']
Q = Tcost['Q']
R = Tcost['R']
## Solution to Riccati Equation
P = DM(scla.solve_discrete_are(np.array(A), np.array(B), \
np.array(Q), np.array(R)))
vfin = 0.5 * (xQx(x, P))
break
elif key == 'vfin_F':
vfin_F = Tcost['vfin_F']
vfin = vfin_F(x, xs)
break
Vfin = Function('Vfin', [x, xs], [vfin])
return Vfin
def fbGain(f, Q=cfg.Q, R=cfg.R):
"""
K = fbGain(f, Q, R) solves the Discrete time
Algebraic Riccatti Equation (DARE) for X:
X = A'XA - (A'XB) * (R+B'XB)^-1 * (B'XA) + Q
Then computes and returns the feedback gain, K:
K = (B'XB + R)^-1 * (B'XA)
The feedback gain can be used to find the control input:
u = -K * (f.X - (leader.X - r))
which is the control input required for the followers to
track a leader to within a pre-specified distance, r,
f : agent
Q : cost matrix
R : cost matrix
"""
X = linalg.solve_discrete_are( f.A, f.B, Q, R )
K = linalg.inv( f.B.T * X * f.B + R ) * ( f.B.T * X * f.A )
return K
def propagate(self, vector):
"""
:param vector: the current deviation of the goal state
:param idx: the current horizon at which the dynamics are estimated
:return: the next state of the system and an update to control matrix
"""
deviation_from_goal = vector.T[0] - self.vehicle.goal.T[0]
theta = vector[-1]
b = self.vehicle.control_matrix(theta)
A = self.vehicle.system_matrix()
P = LA.solve_discrete_are(A, b, self.Q, self.R)
K = np.linalg.inv(self.R).dot(np.dot(b.T, P))
eigVals, eigVecs = LA.eig(A - np.matmul(b, K))
# Project the solver
span = np.linspace(self.t0, self.tf, self.num_pts)
# Drive deviation to zero by solving xdot = (A-BK)x
sol = integrate.solve_ivp(fun=self.sys_func,
t_span=span,
y0=deviation_from_goal,
args=(A, b, K),
method='RK45',
t_eval=span)
# Optimal Trajectory and Control
optimal_trajectory = sol.y
optimal_control = np.matmul(-K, optimal_trajectory)
# Only take the first action and limit the entries
v_clipped = np.clip(optimal_control[0, [0]], self.vehicle.v_min,
self.vehicle.v_max)
omega_clipped = np.clip(optimal_control[1, [0]],
self.vehicle.omega_min, self.vehicle.omega_max)
# Apply control to current state
control = np.array([v_clipped, omega_clipped], dtype=float)
du = self.dT * np.matmul(b, control)
return du, control
def leaderEst(*agents):
"""
xh_new = leaderEst(*agents) is the leader's estimation method.
If only the leader is passed, the leader will estimate its state
only based on its own observation.
If leader and followers are passed, the leader will estimate its
state using its own observations AND the followers' observation.
"""
# Simple Estimation
if len(agents)==1:
l = agents[0]
xh_new = l.A * l.xh_old + l.B * l.u_old + \
l.Ls * ( l.y11 - l.C * l.A * l.xh_old - l.C * l.B * l.u_old )
# Advanced Estimation
if len(agents)>1:
l = agents[0]
f = agents[1][0]
if cfg.leaderEstSwitch:
# Obtain Leader's and Follower's Data
K2 = fbGain(f)
L2 = kalGain(f)
Q1 = np.matrix( l.stateCov )
Q2 = np.matrix( f.stateCov )
R21 = np.matrix( f.sensorCov1 )
R22 = np.matrix( f.sensorCov2 )
X = linalg.solve_discrete_are( f.A.T, f.C.T, Q2, R21 ) # fix this R2
P = (np.matrix(np.identity(4)) - L2*f.C) * X
# Build Augment Matrices
cfg.Abar = np.r_[ np.c_[ l.A , np.zeros_like(l.A) ],
np.c_[ f.B*K2*f.C, f.A-f.B*K2*f.C ] ]
cfg.Bbar = np.r_[ np.c_[ l.B , np.zeros((4,4)) ],
np.c_[ np.zeros_like(l.B), f.B*K2 ] ]
cfg.Cbar = np.r_[ np.c_[ l.C , np.zeros_like(l.C) ],
np.c_[ np.zeros_like(l.C), l.C ] ]
cfg.Qbar = np.r_[ np.c_[ Q1 , np.zeros_like(Q1) ],
np.c_[ np.zeros_like(Q1), Q2+f.B*K2*f.C*P*f.C.T*\
K2.T*f.B.T+f.B*K2*R22*K2.T*f.B.T ] ]
cfg.Rbar = np.r_[ np.c_[ R21 , np.zeros_like(R21) ],
np.c_[ np.zeros_like(R21), R22 ] ]
# Compute the Augmented Kalman Gain
Xbar = linalg.solve_discrete_are( cfg.Abar.T, cfg.Cbar.T, cfg.Qbar, cfg.Rbar )
l.Lbar = Xbar * cfg.Cbar.T * linalg.inv( cfg.Cbar*Xbar*cfg.Cbar.T + cfg.Rbar )
# Initialize Augmented Estimate
l.xh_old_bar = np.matrix(np.zeros((8,1)))
# Turn Off Switch
cfg.leaderEstSwitch = 0
# Calculate New Estimate
ubar = np.r_[ l.u_old, cfg.r[:,0], np.matrix(np.zeros((2,1))) ]
l.xh_new_bar = cfg.Abar * l.xh_old_bar + \
cfg.Bbar * ubar + \
l.Lbar * ( np.r_[ l.y11, l.y12 ] -
cfg.Cbar * cfg.Abar * l.xh_old_bar -
cfg.Cbar * cfg.Bbar * ubar )
# Update Old Estimate
l.xh_old_bar = l.xh_new_bar
xh_new = l.xh_new_bar[0:4,0]
return xh_new
def dlqr(self, A, B, Q, R):
P = np.matrix(sp_linalg.solve_discrete_are(A, B, Q, R))#solve discrete time ricatti equation
K = np.matrix(sp_linalg.inv(B.T*P*B+R)*(B.T*P*A))#compute the LQR gain
eigVals, eigVecs = sp_linalg.eig(A-B*K)
return K, eigVals
