def __init__(self, plant, x_eq, u_eq, y_eq, controller_poles,
                 estimator_poles):
        """Creates a new inverted pendulum controller.

        Args:
            plant: The plant being controlled.
            x_eq: The equilibrium state (or operating point).
            u_eq: The input required to maintain equilibrium.
            y_eq: The sensed output in the equilibrium state.
            controller_poles: The poles to use for the controller.
            estimator_poles: The poles to use for the estimator.
        """
        self.plant = plant
        self.u_eq = u_eq
        self.y_eq = y_eq
        self.z_hat = [0, 0]

        # Create system matrices.
        self.A = [[0, 1],
                  [(plant.m * constants.g * np.cos(x_eq[0]) -
                    u_eq * np.sin(x_eq[0])) / (plant.m * plant.l),
                   -plant.gamma / (plant.l**2 * plant.m)]]
        self.B = [[0], [np.cos(x_eq[0]) / (plant.m * plant.l)]]
        self.C = [[plant.l * np.cos(x_eq[0]), 0]]
        self.D = [[0]]
        self.K = control.place(self.A, self.B, controller_poles)
        self.L = np.transpose(
            control.place(np.transpose(self.A), np.transpose(self.C),
                          estimator_poles))

        # Precompute frequently used matrices.
        self.A_BK = np.subtract(self.A, np.matmul(self.B, self.K))
        self.C_DK = np.subtract(self.C, np.matmul(self.D, self.K))
Beispiel #2
0
 def set_poles(self, poles):
     poles = np.asarray(poles)
     self.K_cont = np.asarray(control.place(self.A, self.B, poles))
     self.K_disc = np.asarray(control.place(self.A_d, self.B_d, np.exp(poles * self.Ts)))
     self.A_hat = self.A - self.B @ self.K_cont
     self.sys_cont = control.StateSpace(self.A_hat, self.B, self.C, self.D, remove_useless=False)
     self.A_hat_d = self.A_d - self.B_d @ self.K_disc
     self.sys_disc = control.StateSpace(self.A_hat_d, self.B_d, self.C_d, self.D_d, self.Ts, remove_useless=False)
     return self.K_disc
Beispiel #3
0
    def cal_feedback(self, param):
        theta_0 = 0
        b_theta = param.lT / (param.m1 * param.l1**2 + param.m2 * param.l2**2 +
                              param.J1y + param.J2y)
        A_lon = np.zeros((2, 2))
        A_lon[0, 1] = 1
        B_lon = np.zeros((2, 1))
        B_lon[1, 0] = b_theta
        C_lon = np.zeros((1, 2))
        C_lon[0, 0] = 1

        self.Feq = (self.param.m1 * self.param.l1 +
                    self.param.m2 * self.param.l2
                    ) * self.param.g * np.cos(theta_0) / self.param.lT
        JT = self.param.m1 * self.param.l1**2 + self.param.m2 * self.param.l2**2 + self.param.J2z + self.param.m3 * (
            self.param.l3x**2 + self.param.l3y**2)

        A_lat = np.zeros((4, 4))
        A_lat[0, 2] = 1
        A_lat[1, 3] = 1
        A_lat[3, 0] = self.param.lT * self.Feq / (JT + self.param.J1z)
        B_lat = np.zeros((4, 1))
        B_lat[2, 0] = 1 / (self.param.J1x)
        C_lat = np.zeros((2, 4))
        C_lat[0, 0] = 1
        C_lat[1, 1] = 1
        Cr_yaw = np.zeros((1, 4))
        Cr_yaw[0, 1] = 1

        des_char_lon = np.array(
            [1, 2 * self.z_theta * self.wn_theta, self.wn_theta**2])
        #print(ct.ctrb(A_lon,B_lon))
        des_pole_lon = np.roots(des_char_lon)
        self.K_lon = ct.place(A_lon, B_lon, des_pole_lon)
        self.kr_lon = -1 / (np.dot(
            np.dot(C_lon, np.linalg.inv(A_lon - B_lon * self.K_lon)), B_lon))

        print(self.K_lon)
        print(self.kr_lon)

        des_char_lat = np.convolve(
            [1, 2 * self.z_phi * self.wn_phi, self.wn_phi**2],
            [1, 2 * self.z_psi * self.wn_psi, self.wn_psi**2])
        des_pole_lat = np.roots(des_char_lat)
        #(ct.ctrb(A_lat,B_lat))
        self.K_lat = ct.place(A_lat, B_lat, des_pole_lat)
        self.kr_lat = -1 / (np.dot(
            np.dot(Cr_yaw, np.linalg.inv(A_lat - B_lat * self.K_lat)), B_lat))

        print(self.K_lat)
        print(self.kr_lat)
Beispiel #4
0
    def __init__(self, slow_d):
        self.fast_d = 0

        #stay still
        # self.slow_d = -5 + 5*0.2

        #move
        self.slow_d = slow_d

        self.desired_states = np.matrix([[0], [slow_d], [0], [self.fast_d]])
        #define our matrices for the linearized model, which we have
        M = 0.5  #cart mass
        m = 0.2  #pedulum mass
        b = 0.1  #coefficient of friction
        I = 0.006  #inertia
        g = 9.8  #gravity
        l = 0.3  #length of the pendulum
        k = I * (M + m) + M * m * l**2.0
        U2 = (I + m * l**2.0) / k
        U4 = -m * l / k
        TF_2_3 = -(g * m**2.0 * l**2.0) / k
        TF_4_3 = (g * (m + M)) / k
        # TF_4_3 = (m*l*(M+m))*g/k;

        self.B = [[0], [U2], [0], [U4]]
        self.A = [[0, 1, 0, 0], [0, 0, TF_2_3, 0], [0, 0, 0, 1],
                  [0, 0, TF_4_3, 0]]
        self.C = [[1, 0, 0, 0], [0, 0, 1, 0]]
        self.D = [[0], [0]]
        self.sys = control.StateSpace(self.A, self.B, self.C, self.D)
        # R = 1
        # Q = [[20, 0, 0, 0], [0, 0, 0, 0], [0, 0, 3, 0], [0, 0, 0, 0,]]

        #develop our fast dynamic controller (Using pole placement)
        self.K = control.place(self.A, self.B, [-5.1, -5.2, -5.3, -5.4])
        self.K = np.matrix(self.K.tolist()[0])
        print("K = ", np.matrix(self.K))
        print("A = ", np.matrix(self.A))
        # print("B = ",np.matrix(self.B))
        # print(np.linalg.eig(np.subtract(self.A, np.matmul(self.B,self.K))))
        # print(np.subtract(self.A, np.matmul(self.B,self.K)))
        exit()

        self.closed_loop = np.subtract(self.A, np.matmul(self.B, self.K))
        self.K2 = control.place(self.closed_loop, self.B,
                                [-5.1, -5.2, -5.3, -5.4])

        #deevelop our slow dynamic controller (PID)
        self.pid = PID(2, 1, 0.0)
Beispiel #5
0
def full_obs(sys,poles):
    """
    Full order observer of the system sys

    Call:
    obs = full_obs(sys,poles)

    Parameters
    ----------
    sys : System in State Space form
    poles: desired observer poles

    Returns
    -------
    obs: ss
    Observer

    """
    if isinstance(sys, ct.TransferFunction):
        "System must be in state space form"
        return
    a = np.mat(sys.A)
    b = np.mat(sys.B)
    c = np.mat(sys.C)
    d = np.mat(sys.D)
    L = ct.place(a.T,c.T,poles)
    L = np.mat(L).T
    Ao = a-L*c
    Bo = np.hstack((b-L*d,L))
    n = np.shape(Ao)
    m = np.shape(Bo)
    Co = np.eye(n[0],n[1])
    Do = np.zeros((n[0],m[1]))
    obs = ct.StateSpace(Ao,Bo,Co,Do,sys.dt)
    return obs
Beispiel #6
0
def red_obs(sys, T, poles):
    """Reduced order observer of the system sys
    Call:
    obs=red_obs(sys,T,poles)
    Parameters
    ----------
    sys : System in State Space form
    T: Complement matrix
    poles: desired observer poles
    Returns
    -------
    obs: ss
    Reduced order Observer
    """
    if isinstance(sys, TransferFunction):
        "System must be in state space form"
        return
    a = np.mat(sys.A)
    b = np.mat(sys.B)
    c = np.mat(sys.C)
    d = np.mat(sys.D)
    T = np.mat(T)
    P = np.mat(np.vstack((c, T)))
    invP = np.inv(P)
    AA = P * a * invP
    ny = np.shape(c)[0]
    nx = np.shape(a)[0]
    nu = np.shape(b)[1]

    A11 = AA[0:ny, 0:ny]
    A12 = AA[0:ny, ny:nx]
    A21 = AA[ny:nx, 0:ny]
    A22 = AA[ny:nx, ny:nx]

    L1 = place(A22.T, A12.T, poles)
    L1 = np.mat(L1).T

    nn = nx - ny

    tmp1 = np.mat(np.hstack((-L1, np.eye(nn, nn))))
    tmp2 = np.mat(np.vstack((np.zeros((ny, nn)), np.eye(nn, nn))))
    Ar = tmp1 * P * a * invP * tmp2

    tmp3 = np.vstack((np.eye(ny, ny), L1))
    tmp3 = np.mat(np.hstack((P * b, P * a * invP * tmp3)))
    tmp4 = np.hstack((np.eye(nu, nu), np.zeros((nu, ny))))
    tmp5 = np.hstack((-d, np.eye(ny, ny)))
    tmp4 = np.mat(np.vstack((tmp4, tmp5)))

    Br = tmp1 * tmp3 * tmp4

    Cr = invP * tmp2

    tmp5 = np.hstack((np.zeros((ny, nu)), np.eye(ny, ny)))
    tmp6 = np.hstack((np.zeros((nn, nu)), L1))
    tmp5 = np.mat(np.vstack((tmp5, tmp6)))
    Dr = invP * tmp5 * tmp4

    obs = StateSpace(Ar, Br, Cr, Dr, sys.dt)
    return obs
Beispiel #7
0
    def initialize(self, param_value_dict):
        self.k1 = param_value_dict["k1"]
        self.k2 = param_value_dict["k2"]
        self.luenberger_poles = param_value_dict["Luenberger poles"]

        AB = np.array([[0, 1, 0, 0, 0, 0],
                       [0, 0, 1, 0, 0, 0],
                       [0, 0, 0, 1, 0, 0],
                       [0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 1],
                       [0, 0, 0, 0, 0, 0]])

        BB = np.array([[0, 0],
                       [0, 0],
                       [0, 0],
                       [1, 0],
                       [0, 0],
                       [0, 1]])

        CB = np.array([[1, 0, 0, 0, 0, 0],
                       [0, 1, 0, 0, 0, 0],
                       [0, 0, 0, 0, 1, 0],
                       [0, 0, 0, 0, 0, 1]])

        L = place(AB.T, CB.T, self.luenberger_poles).T

        cont_observer_system = ss(AB - L @ CB, np.hstack((BB, L)), np.zeros((1, 6)), np.zeros((1, 6)))
        # TODO: Remove hardcoded sample time
        discrete_observer_system = sample_system(cont_observer_system, 1/60)
        self.luenberger_Ad, self.luenberger_Bd, _, _ = ssdata(discrete_observer_system)
        self.run_once = False
Beispiel #8
0
    def __init__(self):
        self.values = None
        self.boostPercent = 0.0  #boost Percentage 0 - 100
        self.lastz = None  #the tick priors value of position
        self.lastvz = None  #the tick priors value of velocity
        self.lastError = 0.0  #prior error
        self.errorzI = 0.0  #summation of values for integral of error
        self.errorzICOUNTER = 1.0

        #Rocket League physics (using unreal units (uu))
        self.gravity = 650  #uu/s^2

        #State Space matrix coefficients
        self.mass = 14.2
        self.M = 1 / 14.2
        self.A = np.array([[0.0, 1.0], [0.0, 0.0]])
        self.B = np.array([[0.0], [1.0]])
        self.C = np.array([[1, 0], [0, 0]])
        self.D = np.array([[0], [0]])
        self.system = control.ss(self.A, self.B, self.C, self.D, None)
        #print(self.B)
        self.poles = np.array([-2.5, -2.8])
        self.K = control.place(self.A, self.B, self.poles)
        self.k1 = self.K[0, 0]
        self.k2 = self.K[0, 1]
        self.kr = 1.0
Beispiel #9
0
    def __init__(self):
        self.fast_d = 0
        self.slow_d = 2
        #define our matrices for the linearized model, which we have
        M = 0.5  #cart mass
        m = 0.2  #pedulum mass
        b = 0.1  #coefficient of friction
        I = 0.006  #inertia
        g = 9.8  #gravity
        l = 0.3  #length of the pendulum
        k = I * (M + m) + M * m * l**2.0
        U2 = (I + m * l**2.0) / k
        U4 = -m * l / k
        TF_2_3 = -(g * m**2.0 * l**2.0) / k
        TF_4_3 = (g * (m + M)) / k
        # TF_4_3 = (m*l*(M+m))*g/k;

        self.B = [[0], [U2], [0], [U4]]
        self.A = [[0, 1, 0, 0], [0, 0, TF_2_3, 0], [0, 0, 0, 1],
                  [0, 0, TF_4_3, 0]]
        self.C = [[1, 0, 0, 0], [0, 0, 1, 0]]
        self.D = [[0], [0]]
        self.sys = control.StateSpace(self.A, self.B, self.C, self.D)
        # R = 1
        # Q = [[20, 0, 0, 0], [0, 0, 0, 0], [0, 0, 3, 0], [0, 0, 0, 0,]]

        #develop our fast dynamic controller (Using pole placement)
        self.K = control.place(self.A, self.B, [-5.1, -5.2, -5.3, -5.4])
        self.K = np.matrix(self.K.tolist()[0])

        #deevelop our slow dynamic controller (PID)
        self.pid = PID(5, 0.1, 0.05)
        def fsf(t, e, v):
            if t == 0:
                start = time.time()
                A = np.concatenate(
                    (np.concatenate((np.zeros((2, 2)),
                                     np.dot(-1 * model.m_inv,
                                            (model.k0 + model.k2 * (v**2)))),
                                    axis=0),
                     np.concatenate(
                         (np.eye(2), np.dot(-1 * model.m_inv, model.c1 * v)),
                         axis=0)),
                    axis=1)
                B = (np.concatenate((np.zeros((2, 2)), model.m_inv),
                                    axis=0)[:, 1])[..., None]
                self.K = control.place(
                    A, B, [self.eval1, self.eval2, self.eval3, self.eval4])
                end = time.time()

                print(self.K)
                # print(end - start)

            e_transpose = np.array([[e[0]], [e[1]], [e[2]], [e[3]]])
            ans = (-self.K * e_transpose)
            # print(ans[0,0])
            return ans[0, 0]
Beispiel #11
0
    def compute_feedback_matrix(self, e_op):
        A, B = compute_linear_ss(self.model_type, e_op)

        try:
            K = ctr.place(A, B, self.poles)
            return K
        except Exception as e:
            print("Error during pole placement: " + str(e))
Beispiel #12
0
    def place_controller_poles(self, poles):
        """Design a controller that places the closed-loop system poles at the
        given locations.

        Most users should just use design_dlqr_controller(). Only use this if
        you know what you're doing.

        Keyword arguments:
        poles -- a list of compex numbers which are the desired pole locations.
                 Complex conjugate poles must be in pairs.
        """
        self.K = ct.place(self.sysd.A, self.sysd.B, poles)
    def __init__(self, settings):
        file = settings["config file"]
        assert os.path.isfile(file)

        with open(file, "rb") as f:
            data = pickle.load(f)

        if "system" not in data:
            raise ValueError("Config file lacks mandatory settings.")

        self.ss = data["system"]

        self.input_offset = data.get("op_inputs", None)
        self.output_offset = data.get("op_outputs", None)

        if self.input_offset is None:
            self.input_offset = np.zeros((self.ss.B.shape[1], ))
        if len(self.input_offset) != self.ss.B.shape[1]:
            raise ValueError("Provided input offset does not match input "
                             "dimensions.")

        if self.output_offset is None:
            self.output_offset = np.zeros((self.ss.C.shape[0], ))
        if len(self.output_offset) != self.ss.C.shape[0]:
            raise ValueError("Length of provided output offset does not match "
                             "output dimensions ({} != {}).".format(
                len(self.output_offset),
                self.ss.C.shape[0]
            ))

        # add specific "private" settings
        settings.update(input_order=0)
        settings.update(output_dim=self.ss.C.shape[0])
        settings.update(input_type=settings["input source"])
        super().__init__(settings)

        if settings.get("poles", None) is None:
            # pretty useless but hey why not.
            self.feedback = np.zeros((self.ss.B.shape[1], self.ss.A.shape[0]))
        else:
            if self.ss.B.shape[1] == 1:
                # save the control/slycot dependency
                self.feedback = place_siso(self.ss.A,
                                           self.ss.B,
                                           self.settings["poles"])
            else:
                import control
                self.feedback = control.place(self.ss.A,
                                              self.ss.B,
                                              self.settings["poles"])

        self.prefilter = calc_prefilter(self.ss.A, self.ss.B, self.ss.C,
                                        self.feedback)
Beispiel #14
0
    def place_observer_poles(self, poles):
        """Design a controller that places the closed-loop system poles at the
        given locations.

        Most users should just use design_kalman_filter(). Only use this if you
        know what you're doing.

        Keyword arguments:
        poles -- a list of compex numbers which are the desired pole locations.
                 Complex conjugate poles must be in pairs.
        """
        L = ct.place(self.sysd.A.T, self.sysd.C.T, poles).T
        self.kalman_gain = np.linalg.inv(self.sysd.A) @ L
    def __init__(self, settings):
        file = settings["config file"]
        assert os.path.isfile(file)

        with open(file, "rb") as f:
            data = pickle.load(f)

        if "system" not in data:
            raise ValueError("Config file lacks mandatory settings.")

        self.ss = data["system"]

        self.input_offset = data.get("op_inputs", None)
        self.output_offset = data.get("op_outputs", None)

        if self.input_offset is None:
            self.input_offset = np.zeros((self.ss.B.shape[1], ))
        if len(self.input_offset) != self.ss.B.shape[1]:
            raise ValueError("Provided input offset does not match input "
                             "dimensions.")

        if self.output_offset is None:
            self.output_offset = np.zeros((self.ss.C.shape[0], ))
        if len(self.output_offset) != self.ss.C.shape[0]:
            raise ValueError("Length of provided output offset does not match "
                             "output dimensions ({} != {}).".format(
                                 len(self.output_offset), self.ss.C.shape[0]))

        # add specific "private" settings
        settings.update(input_order=0)
        settings.update(output_dim=self.ss.C.shape[0])
        settings.update(input_type=settings["input source"])
        super().__init__(settings)

        if settings.get("poles", None) is None:
            # pretty useless but hey why not.
            self.feedback = np.zeros((self.ss.B.shape[1], self.ss.A.shape[0]))
        else:
            if self.ss.B.shape[1] == 1:
                # save the control/slycot dependency
                self.feedback = place_siso(self.ss.A, self.ss.B,
                                           self.settings["poles"])
            else:
                import control
                self.feedback = control.place(self.ss.A, self.ss.B,
                                              self.settings["poles"])

        self.prefilter = calc_prefilter(self.ss.A, self.ss.B, self.ss.C,
                                        self.feedback)
Beispiel #16
0
def calc_control_parameters(M, m, b, l, I):
    """Given a system description, calculate a control matrix"""
    p = I * (M + m) + M * m * l**2  # denominator for the A and B matrices
    g = gravity

    A = array([[0, 1, 0, 0],
               [0, -(I + m * l**2) * b / p, (m**2 * g * l**2) / p, 0],
               [0, 0, 0, 1], [0, -(m * l * b) / p, m * g * l * (M + m) / p,
                              0]])
    B = array([[0], [(I + m * l**2) / p], [0], [m * l / p]])

    # Place arbitrary negative poles in the control matrix as described.
    K = control.place(A, B, [-1, -1, -1, -1])

    return K[0]
Beispiel #17
0
    def __init__(self):
        self.values = None
        self.boostPercent = 0.0  #boost Percentage 0 - 100
        self.pitchPercent = 0  #pitch percentage
        self.lastz = None  #the tick priors value of position
        self.lastvz = None  #the tick priors value of velocity
        self.lastError = 0.0  #prior error
        self.errorzI = 0.0  #summation of values for integral of error
        self.errorzICOUNTER = 1.0

        #Rocket League physics (using unreal units (uu))
        self.g = 650  #uu/s^2
        self.Dp = -2.798194258050845  #Drag coeff for pitch
        self.Tp = 12.14599781908070
        T_r = -36.07956616966136
        # torque coefficient for roll
        T_p = -12.14599781908070
        # torque coefficient for pitch
        T_y = 8.91962804287785
        # torque coefficient for yaw
        D_r = -4.47166302201591
        # drag coefficient for roll
        D_p = -2.798194258050845
        # drag coefficient for pitch
        D_y = -1.886491900437232
        # drag coefficient for yaw
        self.I = 1
        self.m = 180  #mass of the car arbitrary units

        #State Space matrix coefficients
        self.A = np.matrix([[0, 1], [0, 0]])
        self.B = np.matrix([[0], [self.Tp]])
        self.C = np.matrix([[1, 0], [0, 1]])
        self.D = np.matrix([[0], [0]])
        self.system = control.ss(self.A, self.B, self.C, self.D, None)
        self.controllability = control.ctrb(self.A, self.B)
        print("ctrb:", self.controllability)
        #print(self.B)

        #print(control.ctrb(self.A, self.B))
        self.poles = np.array([-1000, -5])
        print('poles: ', self.poles)
        #self.eigen= control.pole(self.system)
        self.eigen = self.system.pole()

        print("Eigen: ", self.eigen)
        self.K = control.place(self.A, self.B, self.poles)
        print("\nK: ", self.K)
Beispiel #18
0
def calc_control_parameters(M,m,b,l,I):
  """Given a system description, calculate a control matrix"""
  p = I*(M+m)+M*m*l**2 # denominator for the A and B matrices
  g = gravity

  A = array([[0,      1,              0,           0],
             [0, -(I+m*l**2)*b/p,  (m**2*g*l**2)/p,   0],
             [0,      0,              0,           1],
             [0, -(m*l*b)/p,       m*g*l*(M+m)/p,  0]])
  B = array([[0],
             [(I+m*l**2)/p],
             [0],
             [m*l/p]])

  # Place arbitrary negative poles in the control matrix as described.
  K = control.place(A,B, [-1,-1,-1,-1])

  return K[0]
Beispiel #19
0
 def _find_act(self):
     ral_sys  = self.fsf_dict['ral_sys']
     ral_poles = ral_sys.pole()
     print(' '.join(('OL RAL poles:', str(ral_poles))))
     new_poles = ral_poles
     for i in range(len(new_poles)):
         if new_poles[i].real > 0:
             new_poles[i] = - new_poles[i].real + 1j * new_poles[i].imag
         elif new_poles[i].real < 0:
             new_poles[i] = new_poles[i].real + 1j * new_poles[i].imag
         elif new_poles[i].real == 0:
             new_poles[i] = - 10 * abs(np.random.random(1)) + 1j * new_poles[i].imag
     print(' '.join(('CL RAL poles:', str(new_poles))))
     K = control.place(ral_sys.A, ral_sys.B, new_poles)
     #Closed-loop state space
     cl_ral_sys = control.StateSpace(ral_sys.A - ral_sys.B @ K, np.zeros(ral_sys.B.shape), ral_sys.C, np.zeros(ral_sys.D.shape))
     cl_ral_poles = cl_ral_sys.pole()
     #Update FSF dictionary
     self.fsf_dict.update(cl_ral_sys = cl_ral_sys)
     self.fsf_dict.update(K = K)
def simulate(dt=0.1):
    import control
    time = np.arange(0, 20, dt)
    vehicle = BicycleVehicle(road=None, position=[0, 5], velocity=8.3)
    xx, uu = [], []
    from highway_env.interval import LPV
    A, B = vehicle.full_lateral_lpv_dynamics()
    K = -np.asarray(control.place(A, B, -np.arange(1, 5)))
    lpv = LPV(x0=vehicle.state[[1, 2, 4, 5]].squeeze(),
              a0=A,
              da=[np.zeros(A.shape)],
              b=B,
              d=[[0], [0], [0], [1]],
              omega_i=[[0], [0]],
              u=None,
              k=K,
              center=None,
              x_i=None)

    for t in time:
        # Act
        u = K @ vehicle.state[[1, 2, 4, 5]]
        omega = 2 * np.pi / 20
        u_p = 0 * np.array([[-20 * omega * np.sin(omega * t) * dt]])
        u += u_p
        # Record
        xx.append(
            np.array(
                [vehicle.position[0], vehicle.position[1],
                 vehicle.heading])[:, np.newaxis])
        uu.append(u)
        # Interval
        lpv.set_control(u, state=vehicle.state[[1, 2, 4, 5]])
        lpv.step(dt)
        x_i_t = lpv.change_coordinates(lpv.x_i_t, back=True, interval=True)
        # Step
        vehicle.act({"acceleration": 0, "steering": u})
        vehicle.step(dt)

    xx, uu = np.array(xx), np.array(uu)
    plot(time, xx, uu)
Beispiel #21
0
    def control(self, t, x, e_traj, lambda_traj):
        # delete front and back speed because we dont need it here
        x = x[:6]
        # p, e, lamb, dp, de, dlamb = x
        # u_op = np.array([self.Vf_op, self.Vb_op])
        # x_op = np.array([0, self.operating_point[1], self.operating_point[0], 0, 0, 0])

        operating_point, Vf_op, Vb_op = get_current_operating_point(
            self.flatness_model_type, e_traj, lambda_traj)
        A, B, Vf_op, Vb_op = getLinearizedMatrices(
            self.linearization_model_type, operating_point, Vf_op, Vb_op)

        linearized_state = x - operating_point

        # compute K for this time step
        try:
            K_now = ctr.place(A, B, self.poles)
        except Exception as e:
            print("Error during pole placement: " + str(e))

        u = -K_now @ linearized_state
        return u
Beispiel #22
0
    def __init__(self, model):

        # Bind model
        self.model = model

        # Desired x_pos
        self.xd = 0.0

        # Compute desired K
        if self.model.name == 'Pendulum':
            desired_eigenvalues = [-2, -8, -9, -10]
        else:
            desired_eigenvalues = [-9, -10]
        self.K = control.place(self.model.A_cont, self.model.B_cont,
                               desired_eigenvalues)

        # Closed loop system matrix
        Acl = self.model.A_cont - self.model.B_cont * self.K

        # DC gain
        Kdc = self.model.C * np.linalg.inv(Acl) * self.model.B_cont

        # Reference gain
        self.Kr = -1 / Kdc[0]
Beispiel #23
0

############ ODE solver for Observer  ########
def obser(x_hat, t, L_, y_):
    u = -np.matmul(K[0], x_hat.reshape(-1, 1))
    dxdt = np.matmul(
        (A - np.matmul(L_, C)), x_hat.reshape(-1, 1)) + B * u + np.matmul(
            L_, y_.reshape((-1, 1)))
    return dxdt.reshape(-1)


############# Solving non linear equation ############################
x0 = np.array([[0], [0], [-0.2], [0], [0.3], [0]])
t = np.arange(0, 100, 0.1)
####################################################################
L = control.place(np.matrix.transpose(A), np.matrix.transpose(C), K[2])
L_ = np.matrix.transpose(L)
x_hat0 = x0.reshape(6)
u = -np.matmul(K[0], x_hat0)
observed = []
observed.append(x_hat0)

for i in range(len(t) - 1):
    print("solving " + str(i))
    X = odeint(pend_non_linear, x_hat0, t[i:i + 2], args=(A, B, K[0]))
    y = np.matmul(C, (X[1]).reshape(-1, 1)).reshape(-1) + np.random.normal(
        mean, std_dev)
    x_hat = odeint(obser, x_hat0, t[i:i + 2], args=(L_, y))
    x_hat0 = x_hat[1]
    u = -np.matmul(K[0], x_hat0)
    observed.append(x_hat0)
Beispiel #24
0
    def __init__(self):

        # get parameters
        try:
            param_namespace = '/whirlybird'
            self.param = rospy.get_param(param_namespace)
        except KeyError:
            rospy.logfatal('Parameters not set in ~/whirlybird namespace')
            rospy.signal_shutdown('Parameters not set')

        g = self.param['g']
        l1 = self.param['l1']
        l2 = self.param['l2']
        m1 = self.param['m1']
        m2 = self.param['m2']
        d = self.param['d']
        h = self.param['h']
        r = self.param['r']
        Jx = self.param['Jx']
        Jy = self.param['Jy']
        Jz = self.param['Jz']
        km = self.param['km']
        Jm = m1*l1**2 + m2*l2**2
        self.Fe = (m1*l1 - m2*l2)*g/l1 #Note this is not the correct value for Fe, you will have to find that yourself


        # Controller
        self.controller = 'SS'

        if self.controller == 'SS':
            self.A_lat = [[0,0,1,0,0],[0,0,0,1,0],[0,0,0,0,0],[l1*self.Fe/(Jm+Jz),0,0,0,0],[0,-1,0,0,0]]
            self.B_lat = [[0],[0],[1/Jx],[0],[0]]
            self.C_lat = [[1,0,0,0],[0,1,0,0]]
            if 5 != np.linalg.matrix_rank(control.ctrb(self.A_lat,self.B_lat)):
                print 'Not controlable!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'



            self.A_lon = [[0,1,0],[(m1*l1-m2*l2)*g*np.sin(0)/(Jm + Jy),0,0],[-1,0,0]]
            self.B_lon = [[0],[l1/(Jm + Jy)],[0]]
            self.C_lon = [[1,0]]
            if 3 != np.linalg.matrix_rank(control.ctrb(self.A_lon,self.B_lon)):
                print 'Not controlable!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'

            # Pitch Gains
            t_r_theta = 1.4
            zeta_theta = 0.707
            w_n_theta = 2.2/t_r_theta
            p_i_lon = -5#-w_n_theta/2.0
            theta_poles = np.roots([1,2*zeta_theta*w_n_theta,w_n_theta**2])
            poles_lon = []
            for pole in theta_poles:
                poles_lon.append(pole)
            poles_lon.append(p_i_lon)
            K = control.place(self.A_lon,self.B_lon,poles_lon)
            self.kr_lon = K[0,2]
            self.K_lon = K[0,0:2]
            #self.kr_lon = -1/(np.matmul(self.C_lon,np.matmul(np.linalg.inv(np.subtract(self.A_lon,np.matmul(self.B_lon,self.K_lon))),self.B_lon)))

            # Yaw Gains
            t_r_phi = 0.3
            zeta_phi = 0.707
            M = 5
            t_r_psi = M*t_r_phi
            zeta_psi = 0.707
            w_n_phi = 2.2/t_r_phi
            w_n_psi = 2.2/t_r_psi
            p_i_lat = -5#-w_n_psi/2.0
            phi_poles = np.roots([1,2*zeta_phi*w_n_phi,w_n_phi**2])
            psi_poles = np.roots([1,2*zeta_psi*w_n_psi,w_n_psi**2])
            poles_lat = []
            for pole in phi_poles:
                poles_lat.append(pole)
            for pole in psi_poles:
                poles_lat.append(pole)
            poles_lat.append(p_i_lat)
            K = control.place(self.A_lat,self.B_lat,poles_lat)
            self.kr_lat = K[0,4]
            self.K_lat = K[0,0:4]
            #self.kr_lat = -1/(np.matmul(self.C_lat[1],np.matmul(np.linalg.inv(np.subtract(self.A_lat,np.matmul(self.B_lat,self.K_lat))),self.B_lat)))

            # Dirty Derivative gains
            self.sigma_theta = 0.05
            self.sigma_phi = 0.05
            self.sigma_psi = 0.05

            # Initialize
            self.thetao = 0
            self.phio = 0
            self.psio = 0
            self.theta_doto = 0
            self.phi_doto = 0
            self.psi_doto = 0
            self.Int_theta = 0.0
            self.error_theta = 0.0
            self.Int_psi = 0.0
            self.error_psi = 0.0

            self.psi_vlim = 0.05
            self.theta_vlim = 0.05
            self.theta_r = 0;
            self.psi_r = 0;




        elif self.controller == 'PID':

            # Roll Gains
            t_r_phi = 0.3
            zeta_phi = 0.707
            self.sigma_phi = 0.05
            self.phi_r = 0.0
            self.P_phi_ = (2.2/t_r_phi)**2*Jx
            self.I_phi_ = 0.0
            self.D_phi_ = 2*zeta_phi*(2.2/t_r_phi)*Jx
            self.prev_phi = 0.0
            self.Int_phi = 0.0
            self.Dir_phi = 0.0
            self.error_phi = 0.0
            self.phi_vlim = 0.05

            # Pitch Gains
            b_theta = l1/(m1*l1**2 + m2*l2**2 + Jy)
            t_r_theta = 1.0
            zeta_theta = 0.707
            self.sigma_theta = 0.05
            self.theta_r = 0.0
            self.P_theta_ = (2.2/t_r_theta)**2/(b_theta)
            self.I_theta_ = 0.3
            self.D_theta_ = 2*zeta_theta*(2.2/t_r_theta)/(b_theta)
            self.prev_theta = 0.0
            self.Int_theta = 0.0
            self.Dir_theta = 0.0
            self.error_theta = 0.0
            self.theta_vlim = 0.05
            # self.theta_ddot_old = [0.0];

            # Yaw Gains
            M = 10
            t_r_psi = M*t_r_phi
            zeta_psi = 0.707
            Fe = (m1*l1 - m2*l2)*g/l1
            b_psi = l1*Fe/(m1*l1**2 + m2*l2 **2 + Jz)
            self.sigma_psi = 0.05
            self.psi_r = 0.0
            self.P_psi_ = (2.2/t_r_psi)**2/(b_psi)
            self.I_psi_ = 0.01
            self.D_psi_ = 2*zeta_psi*(2.2/t_r_psi)/(b_psi)
            self.prev_psi = 0.0
            self.Int_psi = 0.0
            self.Dir_psi = 0.0
            self.error_psi = 0.0
            self.psi_vlim = 0.05


        self.prev_time = rospy.Time.now()



        self.sat_low = 0.0
        self.sat_high = 0.7

        self.command_sub_ = rospy.Subscriber('whirlybird', Whirlybird, self.whirlybirdCallback, queue_size=5)
        self.psi_r_sub_ = rospy.Subscriber('psi_r', Float32, self.psiRCallback, queue_size=5)
        self.theta_r_sub_ = rospy.Subscriber('theta_r', Float32, self.thetaRCallback, queue_size=5)
        self.command_pub_ = rospy.Publisher('command', Command, queue_size=5)

        while not rospy.is_shutdown():
            # wait for new messages and call the callback when they arrive
            rospy.spin()
Beispiel #25
0
zero_padder = np.array([[0, 0, 0, 0, 0], [0, 0, 0, 0, 0]])
AB = np.concatenate((A, B), axis=1)
mat = np.concatenate((AB, zero_padder), axis=0)
exmat = sp_expm(ts * mat)

Ad = exmat[0:n_states, 0:n_states]
Bd = exmat[0:n_states, n_states:n_states + n_inputs]

print "\n -------------Closed-Loop Control------------- "

eigval, eigvec = np.linalg.eig(A)
print "\neigen values of A = " + str(eigval)

# -12.87243308  -3.64492168  10.86290231
K_matrix = control.place(A, B, [-12.87243308, -3.64492168, -10.86290231])
print "\nK matrix = \n" + str(K_matrix)

A_BK = A - np.dot(B, K_matrix)
print "\nA - BK = \n" + str(A_BK)

eigval_newC, eigvec_newC = np.linalg.eig(A_BK)
print "\neigen values of (A-BK) = " + str(eigval_newC)

print "\n now lets do some simulations:"
t_ = np.zeros((1, iterations))
x_ = np.zeros((n_states, iterations + 1))
z_ = np.zeros((1, iterations + 1))
u_ = np.zeros((n_inputs, iterations + 1))

x0 = np.array([[Xo[0][0]], [Xo[1][0]], [Xo[2][0]]])
Beispiel #26
0
           [0, -(I+m*l**2)*b/p,  (m**2*g*l**2)/p,   0],
           [0,      0,              0,           1],
           [0, -(m*l*b)/p,       m*g*l*(M+m)/p,  0]])
B = array([[0],
           [(I+m*l**2)/p],
           [0],
           [m*l/p]])
C = array([[1, 0, 0, 0],
           [0, 0, 1, 0]])
D = array([[0],
           [0]])

poles,vect = eig(A)
# Print vectors to verify that the system is unstable
print poles

K = control.place(A,B, [-1,-1,-1,-1])
print 'K(place)=',K

Q = C.transpose().dot(C)
Q[1,1]=5000
Q[3,3] = 100
R = 1
K,S,E = control.lqr(A,B,Q,R)
print 'K(lqr)=',K

# New control matrix
Ac = A-B.dot(K)
poles,vect = eig(Ac)
print "Ac poles=", poles
Beispiel #27
0
theta = 0  # pendulum angle from vertical down
g = 9.8  # gravitational constant

p = I * (M + m) + M * m * l**2  # denominator for the A and B matrices

A = array([[0, 1, 0,
            0], [0, -(I + m * l**2) * b / p, (m**2 * g * l**2) / p, 0],
           [0, 0, 0, 1], [0, -(m * l * b) / p, m * g * l * (M + m) / p, 0]])
B = array([[0], [(I + m * l**2) / p], [0], [m * l / p]])
C = array([[1, 0, 0, 0], [0, 0, 1, 0]])
D = array([[0], [0]])

poles, vect = eig(A)
# Print vectors to verify that the system is unstable
print poles

K = control.place(A, B, [-1, -1, -1, -1])
print 'K(place)=', K

Q = C.transpose().dot(C)
Q[1, 1] = 5000
Q[3, 3] = 100
R = 1
K, S, E = control.lqr(A, B, Q, R)
print 'K(lqr)=', K

# New control matrix
Ac = A - B.dot(K)
poles, vect = eig(Ac)
print "Ac poles=", poles
Beispiel #28
0
# Add labels to the figure
normalized_label(inpfig, outfig)
plt.legend(('$\zeta_c = 0.5$', '$\zeta_c = 0.7$',
            '$\zeta_c = 1$'))  # Place a legend on the Axes.
plt.tight_layout()  # Adjust the padding between and around subplots.

# ## Eigenvalue placement observer design (Example 8.3)
# ##
# Find the eigenvalue from the characteristic polynomial
wo = 1  # bandwidth for the observer
zo = 0.7  # damping ratio for the observer
eigs = np.roots([1, 2 * zo * wo, wo**2])

# Compute the estimator gain using eigenvalue placement利用特征值布局计算估计增益
L = np.transpose(ct.place(np.transpose(A), np.transpose(C), eigs))
print("L = ", L)

# Create a linear model of the lateral dynamics driving the estimator
est = ct.StateSpace(A - L @ C, np.block([[B, L]]), np.eye(2), np.zeros((2, 2)))

#
#
#

# ##  Linear observer applied to nonlinear system output
# ##
# Convert the curvy trajectory into normalized coordinates
x_ref = x_curvy[0] / wheelbase
y_ref = x_curvy[1] / wheelbase
theta_ref = x_curvy[2]
Beispiel #29
0
    def synthesize_controller(self,
                              pole_placement=False,
                              ensure_stability=True):
        """
            Synthesize a controller the interval predictor via an LMI
        :param pole_placement: use pole placement to synthesize the controller instead
        :param ensure_stability: whether we need to check stability when the pole placement method is used
        :return: whether we found stabilising controls (True if ensure_stability is False)
        """
        # Input matrices
        A0 = np.array(self.config["A0"])
        dA = np.array(self.config["dA"])
        B = np.array(self.config["B"])
        logger.debug("A0:\n{}".format(A0))
        logger.debug("dA:\n{}".format(dA))
        logger.debug("B:\n{}".format(B))
        dAp = sum(pos(dAi) for dAi in dA)
        dAn = sum(neg(dAi) for dAi in dA)
        DA = sum(dAi for dAi in dA)
        p, q = int(B.shape[0]), int(B.shape[1])

        # Extended matrices
        zero = np.zeros((p, p))
        cA0 = np.concatenate((np.concatenate(
            (A0, zero), axis=1), np.concatenate((zero, A0), axis=1)))
        cA1 = np.concatenate((np.concatenate(
            (zero, -dAn), axis=1), np.concatenate((zero, dAp), axis=1)))
        cA2 = np.concatenate((np.concatenate(
            (-dAp, zero), axis=1), np.concatenate((dAn, zero), axis=1)))
        cB = np.concatenate((B, B))

        # Pole placement
        if pole_placement:
            import control
            logger.debug(
                "The eigenvalues of the matrix A0 = {},  Uncontrollable states = {}"
                .format(np.linalg.eigvals(A0),
                        p - np.linalg.matrix_rank(control.ctrb(A0, B))))
            poles = self.config.get(
                "poles", np.minimum(np.linalg.eigvals(A0),
                                    -np.arange(1, p + 1)))
            K = -control.place(A0, B, poles)
            logger.debug("The eigenvalues of the matrix A0+BK = {}".format(
                np.linalg.eigvals(A0 + B * K)))
            logger.debug("The eigenvalues of the matrix A0+BK+DA = {}".format(
                np.linalg.eigvals(A0 + B * K + DA)))
            logger.debug("The eigenvalues of the matrix A0+BK-DA = {}".format(
                np.linalg.eigvals(A0 + B * K - DA)))
            self.K0 = 0.5 * np.concatenate((K, K), axis=1)
            self.K1 = self.K2 = np.zeros(self.K0.shape)
            cA0 += cB @ self.K0
            if not ensure_stability:
                return True

        # Solve LMI
        success = self.stability_lmi(cA0,
                                     cA1,
                                     cA2,
                                     cB,
                                     synthesize_control=not pole_placement)
        # If control synthesis via LMI fails, try pole placement instead
        if not success and not pole_placement:
            success = self.synthesize_controller(
                pole_placement=True, ensure_stability=ensure_stability)
        return success
Beispiel #30
0
# Statespace equations
A = np.array([[0.0, 1.0], [(-k1 / m), (-b / m)]])
B = np.array([[0.0], [(1 / (m))]])
C = np.array([[1.0, 0.0]])

# Augmented Statespace system for use with integrator full state feedback
A1 = np.array([[0.0, 1.0, 0.0], [(-k1 / m), (-b / m), 0.0], [-1.0, 0.0, 0.0]])
B1 = np.array([[0.0], [(1 / m)], [0.0]])

# gain calculation
des_poles = np.array([-5 + 0.1j, -5 - 0.1j])
polesI = np.append(des_poles, -5)
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
    print('The system is not controllable.')
else:
    K1 = cnt.place(A1, B1, polesI)
    K = np.array([K1.item(0), K1.item(1)])
    ki2 = K1.item(2)

# observer calculation
obsv_poles = 10 * des_poles

# compute gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
    print('The system is not observable.')
else:
    L = cnt.acker(A.T, C.T, obsv_poles).T

print('K: ', K)
print('ki2: ', ki2)
print('L^T: ', L.T)
Beispiel #31
0
            [0,0,0,1.],\
            [0,d1/L,_q,-d2] ] )

B = np.expand_dims(np.array([0, 1.0 / M, 0., -1 / (M * L)]), 1)

# Pendulum Down - Verified correct.
# Eigen Values of this: array([ 0.00+0.j        , -1.00+0.j        , -0.25+2.78881695j,       -0.25-2.78881695j])
# A = np.array([\
#             [0,1,0,0], \
#             [0,-d1, -g*m/(2*m+M),0],\
#             [0,0,0,1],\
#             [0,0,-(M+m)*g/(M*L),-d2] ] )

#B = np.array( [] )

print 'A\n', A
print 'B\n', B

# Controllability
print '---Controllability'
print 'rank of ctrb(A,b)', np.linalg.matrix_rank(control.ctrb(A, B))
print 'Eigenvalues of A ', np.linalg.eig(A)

# Pole Placement
K = control.place(A, B, [-1, -2, -4, -5])
print '---Pole Placement\nK=\n', K

# Verification of Eigen values of A-BK
print '---Verification of Eigen values of A-BK'
print 'Eigenvalues of A-BK', np.linalg.eig(A - np.matmul(B, K))
Beispiel #32
0
              [2, -1]])

B = np.array([[1],
              [0]])
C = np.array([1, 0])
D = np.array([0])

sys = c.ss(A, B, C, D)  # Create state-space model of sample system
stepU = c.step_response(sys)

E = LA.eigvals(A)
print(E)# One of the eigenvalues is >0 so the energy of the system will blow up to infty
P = np.array([-2, -1])  # Place poles at -2, -1
# Note: You can change the aggressiveness by changing the poles

K = c.place(A, B, P)  # Place new poles

Acl = np.array(A - B * K)  # Create a closed loop A matrix
Ecl = LA.eigvals(Acl)  # Calculate new eigenvalues (this evaluates to the poles that I placed!)

syscl = c.ss(Acl, B, C, D)  # Closed loop state space model

step = c.step_response(syscl)

Kdc = c.dcgain(syscl)  # Calculating a dc gain in order to reach intended output goal
Kr = 1 / Kdc


sysclNew = c.ss(Acl, B * Kr, C, D)

sysclNewDiscrete = matlab.c2d(sysclNew, 0.01)
Beispiel #33
0
def main():
    J = 7.7500e-05
    b = 8.9100e-05
    Kt = 0.0184
    Ke = 0.0211
    R = 0.0916
    L = 5.9000e-05

    # fmt: off
    A = np.array([[-b / J, Kt / J], [-Ke / L, -R / L]])
    B = np.array([[0], [1 / L]])
    C = np.array([[1, 0]])
    D = np.array([[0]])
    # fmt: on

    sysc = cnt.StateSpace(A, B, C, D)

    dt = 0.0001
    tmax = 0.025

    sysd = sysc.sample(dt)

    # fmt: off
    Q = np.array([[1 / 20**2, 0], [0, 1 / 40**2]])
    R = np.array([[1 / 12**2]])
    # fmt: on
    K_pp1 = cnt.place(sysd.A, sysd.B, [0.1, 0.9])
    K_pp2 = cnt.place(sysd.A, sysd.B, [0.1, 0.8])
    K_lqr = frccnt.lqr(sysd, Q, R)

    t = np.arange(0, tmax, dt)
    r = np.array([[2000 * 0.1047], [0]])
    r_rec = np.zeros((2, 1, len(t)))

    # Pole placement 1
    x_pp1 = np.array([[0], [0]])
    x_pp1_rec = np.zeros((2, 1, len(t)))
    u_pp1_rec = np.zeros((1, 1, len(t)))

    # Pole placement 2
    x_pp2 = np.array([[0], [0]])
    x_pp2_rec = np.zeros((2, 1, len(t)))
    u_pp2_rec = np.zeros((1, 1, len(t)))

    # LQR
    x_lqr = np.array([[0], [0]])
    x_lqr_rec = np.zeros((2, 1, len(t)))
    u_lqr_rec = np.zeros((1, 1, len(t)))

    u_min = np.asarray(-12)
    u_max = np.asarray(12)

    for k in range(len(t)):
        # Pole placement 1
        u_pp1 = K_pp1 @ (r - x_pp1)

        # Pole placement 2
        u_pp2 = K_pp2 @ (r - x_pp2)

        # LQR
        u_lqr = K_lqr @ (r - x_lqr)

        u_pp1 = np.clip(u_pp1, u_min, u_max)
        x_pp1 = sysd.A @ x_pp1 + sysd.B @ u_pp1
        u_pp2 = np.clip(u_pp2, u_min, u_max)
        x_pp2 = sysd.A @ x_pp2 + sysd.B @ u_pp2
        u_lqr = np.clip(u_lqr, u_min, u_max)
        x_lqr = sysd.A @ x_lqr + sysd.B @ u_lqr

        r_rec[:, :, k] = r
        x_pp1_rec[:, :, k] = x_pp1
        u_pp1_rec[:, :, k] = u_pp1
        x_pp2_rec[:, :, k] = x_pp2
        u_pp2_rec[:, :, k] = u_pp2
        x_lqr_rec[:, :, k] = x_lqr
        u_lqr_rec[:, :, k] = u_lqr

    plt.figure(1)

    plt.subplot(3, 1, 1)
    plt.plot(t, r_rec[0, 0, :], label="Reference")
    plt.ylabel("$\omega$ (rad/s)")
    plt.plot(t,
             x_pp1_rec[0, 0, :],
             label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
    plt.plot(t,
             x_pp2_rec[0, 0, :],
             label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
    plt.plot(t, x_lqr_rec[0, 0, :], label="LQR")
    plt.legend()

    plt.subplot(3, 1, 2)
    plt.plot(t, r_rec[1, 0, :], label="Reference")
    plt.ylabel("Current (A)")
    plt.plot(t,
             x_pp1_rec[1, 0, :],
             label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
    plt.plot(t,
             x_pp2_rec[1, 0, :],
             label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
    plt.plot(t, x_lqr_rec[1, 0, :], label="LQR")
    plt.legend()

    plt.subplot(3, 1, 3)
    plt.plot(t,
             u_pp1_rec[0, 0, :],
             label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
    plt.plot(t,
             u_pp2_rec[0, 0, :],
             label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
    plt.plot(t, u_lqr_rec[0, 0, :], label="LQR")
    plt.legend()
    plt.ylabel("Control effort (V)")
    plt.xlabel("Time (s)")

    if "--noninteractive" in sys.argv:
        latexutils.savefig("case_study_pp_lqr")
    else:
        plt.show()