def __init__(self,
A: jnp.ndarray,
B: jnp.ndarray,
Q: jnp.ndarray = None,
R: jnp.ndarray = None) -> None:
"""
Description: Initialize the infinite-time horizon LQR.
Args:
A (jnp.ndarray): system dynamics
B (jnp.ndarray): system dynamics
Q (jnp.ndarray): cost matrices (i.e. cost = x^TQx + u^TRu)
R (jnp.ndarray): cost matrices (i.e. cost = x^TQx + u^TRu)
Returns:
None
"""
state_size, action_size = B.shape
if Q is None:
Q = jnp.identity(state_size, dtype=jnp.float32)
if R is None:
R = jnp.identity(action_size, dtype=jnp.float32)
# solve the ricatti equation
X = dare(A, B, Q, R)
# compute LQR gain
self.K = jnp.linalg.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
def _dlqr(self, A, B, Q, R):
# solve the ricatti equation for P
P = dare(A, B, Q, R)
# solve for our feedback matrix
# K = (B.T P B + R)^-1 (B.T P A)
K = np.linalg.multi_dot([np.linalg.inv(np.linalg.multi_dot([B.T, P, B]) + R), B.T, P, A])
return P, K
def computeStationaryCovariance(self, ls):
'''Compute the asymptotic covariance for a given linear system.
(Eq. 104-106)
'''
A, H, G, Q, M, R = map(np.mat, [ls.getA(), ls.getH(), ls.getG(),
ls.getQ(), ls.getM(), ls.getR()])
try:
Pprd = np.mat(dare(A.T, H.T, G * Q * G.T, M * R * M.T))
except:
raise AssertionError("Dare Unsolvable: "
+ "The given system state is not stable/observable.")
assert np.all(np.isreal(Pprd)) and np.all(Pprd == Pprd.T)
Pest = Pprd - (Pprd * H.T) * (H * Pprd * H.T + M * R * M.T).I * (Pprd * H.T).T
assert np.all(np.isreal(Pest)) and np.all(Pest == Pest.T)
return Pest
def __init__(self, A, B, Q = None, R = None):
"""
Description: Initialize the infinite-time horizon LQR.
Args:
A, B (float/numpy.ndarray): system dynamics
Q, R (float/numpy.ndarray): cost matrices (i.e. cost = x^TQx + u^TRu)
"""
n, m = B.shape # State & Action Dimensions
if(Q is None or type(Q)):
Q = onp.identity(n, dtype=np.float32)
if(R is None):
R = onp.identity(m, dtype=np.float32)
# solve the ricatti equation
X = dare(A, B, Q, R)
#compute LQR gain
self.K = np.linalg.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
def __init__(self, motionModel, observationModel, target,
stateWeight, controlWeight):
super(SLQRController, self).__init__(motionModel, observationModel)
self.target = target
self.Wx = stateWeight
self.Wu = controlWeight
# compute gain matrix
u_zero = self.motionModel.getZeroControl()
A = np.mat(self.motionModel.getStateJacobian(self.target, u_zero))
B = np.mat(self.motionModel.getControlJacobian(self.target, u_zero))
try:
S = np.mat(dare(A, B, self.Wx, self.Wu))
except:
raise AssertionError("Dare Unsolvable: "
+ "The given system state is not controllable.")
return None
# ensure that it is a real symmetric matrix
assert np.all(S == S.T) and (np.all(np.isreal(S)))
self.L = np.asarray((B.T * S * B + self.Wu).I * B.T * S * A)
self.ls = LinearSystem(self.target, self.motionModel.getZeroControl(),
self.motionModel, self.observationModel)
def computeStationaryCovariance(self, ls):
'''Compute the asymptotic covariance for a given linear system.
(Eq. 104-106)
'''
A, H, G, Q, M, R = map(
np.mat,
[ls.getA(),
ls.getH(),
ls.getG(),
ls.getQ(),
ls.getM(),
ls.getR()])
try:
Pprd = np.mat(dare(A.T, H.T, G * Q * G.T, M * R * M.T))
except:
raise AssertionError(
"Dare Unsolvable: " +
"The given system state is not stable/observable.")
assert np.all(np.isreal(Pprd)) and np.all(Pprd == Pprd.T)
Pest = Pprd - (Pprd * H.T) * (H * Pprd * H.T + M * R * M.T).I * (Pprd *
H.T).T
assert np.all(np.isreal(Pest)) and np.all(Pest == Pest.T)
return Pest
def test_unstable_system(self):
T = 15
Ts = 1e-1
A = [[1.178, 0.001, 0.511, -0.403], [-0.051, 0.661, -0.011, 0.061],
[0.076, 0.335, 0.560, 0.382], [0., 0.335, 0.089, 0.849]]
B = [[0.004, -0.087], [0.467, 0.001], [0.213, -0.235], [0.213, -0.016]]
A = np.array(A)
B = np.array(B)
n, m = B.shape
sys = scipysig.StateSpace(A,
B,
np.identity(n),
np.zeros((n, m)),
dt=Ts)
u = np.random.randn(T + 1, m)
_, _, x = scipysig.dlsim(sys, u)
Qs = [np.identity(n), 0 * np.identity(n)]
Rs = [np.identity(m), 0 * np.identity(m)]
for Q in Qs:
for R in Rs:
if np.all(Q == Qs[1]):
with self.assertRaises(ValueError):
K, V = optimal_control(x, u, Q, R)
else:
# Solve optimal control problem with Riccati equation
P = dare(A, B, Q, R)
trueK = -np.linalg.inv(B.T @ P @ B + R) @ B.T @ P @ A
# Solve using Willems' lemma
K, V = optimal_control(x, u, Q, R)
# Compare results
self.assertTrue(np.linalg.norm(K - trueK) < 1e-4)
def __init__(self, motionModel, observationModel, target,
stateWeight, controlWeight):
super(OffAxisSLQRController, self).__init__(motionModel, observationModel)
self.d = 0.1 #TODO: load from mission file
self.target = target
self.offaxis_target = self.getOffAxis(self.target)
self.Wx = stateWeight[:2, :2]
self.Wu = controlWeight[:2, :2]
# compute gain matrix
A = np.mat(np.eye(2))
B = np.mat(self.motionModel.dt * np.eye(2))
try:
S = np.mat(dare(A, B, self.Wx, self.Wu))
except:
print A, B, self.Wx, self.Wu
raise AssertionError("Dare Unsolvable: "
+ "The given system state is not controllable.")
return None
# ensure that it is a real symmetric matrix
assert np.all(S == S.T) and (np.all(np.isreal(S)))
self.L = np.asarray((B.T * S * B + self.Wu).I * B.T * S * A)
self.ls = LinearSystem(self.target, self.motionModel.getZeroControl(),
self.motionModel, self.observationModel)
np.maximum(np.abs(n*T_max*np.cos(phi)*np.cos(alpha_max)+m_wet*g),
np.abs(n*tau*T_max*np.cos(phi)+m_wet*g))])
# Unscaled weight
Iq = np.eye(n)
Iq[-1,-1] = 0.
Qhat = [Iq,10*Iq]
Rhat = np.eye(m)
# Scaled weights
Q = [la.inv(Dx.T).dot(Qhat_i).dot(la.inv(Dx)) for Qhat_i in Qhat]
R = la.inv(Du.T).dot(Rhat).dot(la.inv(Du))
K, P, A_cl = [], [], []
for Qi in Q:
P.append(dare(A,B,Qi,R))
K.append(-la.inv(R+B.T.dot(P[-1]).dot(B)).dot(B.T).dot(P[-1]).dot(A))
A_cl.append(A+B.dot(K[-1]))
if False:
# Plot closed-loop eigenvalues
spec_cl = [la.eigvals(A_cl_i) for A_cl_i in A_cl]
fig = plt.figure(1,figsize=(6,6))
plt.clf()
ax = fig.add_subplot(111)
ax.add_artist(plt.Circle((0, 0), 1, facecolor='none', edgecolor='black'))
for i in range(len(spec_cl)):
ax.plot(np.real(spec_cl[i]), np.imag(spec_cl[i]),
marker='x', linestyle='none', label=('$Q=%d\cdot I$'%(int(np.max(Qhat[i])))))
ax.axis('equal')
ax.set_xlim([-1.2,1.2])
def __init__(self, A, B, Q, R):
P = dare(A, B, Q, R)
self.K = np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)
def updateController(self, A, B, Q, R):
S = np.asmatrix(dare(A, B, Q, R)) #from scipy
K = -(R + B.T * S * B).I * B.T * S * A # neg for neg feedback
return K
import jax
import jax.numpy as np
import numpy.random as random
import matplotlib.pyplot as plt
from scipy.linalg import solve_discrete_are as dare
n, m, A, B = 2, 1, np.array([[1., 1.], [0., 1.]]), np.array([[0.], [1.]])
Q, R = np.eye(N = n), np.eye(N = m)
x0 = np.zeros((n, 1))
T, H, M, lr, steps = 200, 10, 10, 0.001, 1
W = random.normal(size = (T, n, 1))
W = np.sin(np.arange(T)/(2*np.pi)).reshape((T, 1)) @ np.ones((1,2))
P = dare(A, B, Q, R)
K = np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)
x, loss_lqr = x0, []
for t in range(T):
u = - K @ x
loss_lqr.append((x.T @ Q @ x + u.T @ R @ u)[0][0])
x = A @ x + B @ u + W[t]
def counterfact_loss(E, W):
y, cost = np.zeros((n, 1)), 0
for h in range(H):
v = - K @ y + np.tensordot(E, W[h : h+M], axes = ([0, 2], [0, 1]))
cost += (y.T @ Q @ y + v.T @ R @ v)[0][0]
y = A @ y + B @ v + W[h+M]
return cost