Пример #1
0
    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)
Пример #2
0
 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
Пример #3
0
 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
Пример #4
0
    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)
Пример #5
0
 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)
Пример #6
0
 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
Пример #7
0
    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)
Пример #8
0
 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)
Пример #9
0
              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])
Пример #10
0
 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)
Пример #11
0
 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
Пример #12
0
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