示例#1
0
def reachable_form(xsys):
    # Check to make sure we have a SISO system
    if not control.issiso(xsys): 
        raise control.ControlNotImplemented(
            "Canonical forms for MIMO systems not yet supported")

    # Create a new system, starting with a copy of the old one
    zsys = control.StateSpace(xsys)

    # Generate the system matrices for the desired canonical form
    zsys.B = zeros(shape(xsys.B)); zsys.B[0, 0] = 1;
    zsys.A = zeros(shape(xsys.A))
    Apoly = poly(xsys.A)                # characteristic polynomial
    for i in range(0, xsys.states):
        zsys.A[0, i] = -Apoly[i+1] / Apoly[0]
        if (i+1 < xsys.states): zsys.A[i+1, i] = 1
    
    # Compute the reachability matrices for each set of states
    Wrx = control.ctrb(xsys.A, xsys.B)
    Wrz = control.ctrb(zsys.A, zsys.B)

    # Transformation from one form to another
    Tzx = Wrz * inv(Wrx)

    # Finally, compute the output matrix
    zsys.C = xsys.C * inv(Tzx)

    return zsys, Tzx
示例#2
0
def reachable_form(xsys):
    # Check to make sure we have a SISO system
    if not control.issiso(xsys):
        raise control.ControlNotImplemented(
            "Canonical forms for MIMO systems not yet supported")

    # Create a new system, starting with a copy of the old one
    zsys = control.StateSpace(xsys)

    # Generate the system matrices for the desired canonical form
    zsys.B = zeros(shape(xsys.B))
    zsys.B[0, 0] = 1
    zsys.A = zeros(shape(xsys.A))
    Apoly = poly(xsys.A)  # characteristic polynomial
    for i in range(0, xsys.states):
        zsys.A[0, i] = -Apoly[i + 1] / Apoly[0]
        if (i + 1 < xsys.states): zsys.A[i + 1, i] = 1

    # Compute the reachability matrices for each set of states
    Wrx = control.ctrb(xsys.A, xsys.B)
    Wrz = control.ctrb(zsys.A, zsys.B)

    # Transformation from one form to another
    Tzx = Wrz * inv(Wrx)

    # Finally, compute the output matrix
    zsys.C = xsys.C * inv(Tzx)

    return zsys, Tzx
示例#3
0
    def __init__(self, A, B, C, Qr, Rr, Qe, Re):
        self.A = A
        self.B = B
        self.C = C
        self.Qr = Qr
        self.Rr = Rr
        self.Qe = Qe
        self.Re = Re

        self.nx = A.shape[0]
        self.nu = B.shape[1]
        self.ny = C.shape[0]

        ###########################################
        # Infinite-horizon discrete-time LQR design
        ###########################################
        ctrb_rank = np.linalg.matrix_rank(control.ctrb(A, B))
        #       if ctrb_rank != self.nx:
        #           raise ValueError('The pair A, B must be controllable (rank of controllability matrix is %d instead of %d).' % (ctrb_rank, self.nx))

        if not is_psd(Qr):
            raise ValueError('Qr must be positive semi-definite.')

        if not is_pd(Rr):
            raise ValueError('Rr must be positive definite.')

        # Solve Discrete Algebraic Riccati Equation
        Pr = sp.linalg.solve_discrete_are(A, B, Qr, Rr)

        # Feedback gain
        self.Kr = -np.linalg.inv(Rr + B.conj().T @ Pr @ B) @ (
            B.conj().T @ Pr @ A)
示例#4
0
def dlqr(sys, Q, R):
    """Solves for the optimal discrete-time LQR controller.

    x(n+1) = A * x(n) + B * u(n)
    J = sum(0, inf, x.T * Q * x + u.T * R * u)

    Keyword arguments:
    A -- numpy.array(states x states), The A matrix.
    B -- numpy.array(inputs x states), The B matrix.
    Q -- numpy.array(states x states), The state cost matrix.
    R -- numpy.array(inputs x inputs), The control effort cost matrix.

    Returns:
    numpy.array(states x inputs), K
    """
    m = sys.A.shape[0]

    controllability_rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
    if controllability_rank != m:
        print(
            "Warning: Controllability of %d != %d, uncontrollable state"
            % (controllability_rank, m)
        )

    # P = A.T * P * A - (A.T * P * B) * np.linalg.inv(R + B.T * P * B) *
    #     (B.T * P.T * A) + Q
    P = sp.linalg.solve_discrete_are(a=sys.A, b=sys.B, q=Q, r=R)

    F = np.linalg.inv(R + sys.B.T * P * sys.B) * sys.B.T * P * sys.A
    return F
示例#5
0
def lqr(sys, Q, R):
    """Solves for the optimal linear-quadratic regulator (LQR).

    For a continuous system:
        xdot = A * x + B * u
        J = int(0, inf, x.T * Q * x + u.T * R * u)
    For a discrete system:
        x(n+1) = A * x(n) + B * u(n)
        J = sum(0, inf, x.T * Q * x + u.T * R * u)

    Keyword arguments:
    A -- numpy.array(states x states), The A matrix.
    B -- numpy.array(inputs x states), The B matrix.
    Q -- numpy.array(states x states), The state cost matrix.
    R -- numpy.array(inputs x inputs), The control effort cost matrix.

    Returns:
    numpy.array(states x inputs), K
    """
    m = sys.A.shape[0]

    controllability_rank = np.linalg.matrix_rank(cnt.ctrb(sys.A, sys.B))
    if controllability_rank != m:
        print(
            "Warning: Controllability of %d != %d, uncontrollable state"
            % (controllability_rank, m)
        )

    if sys.dt == 0.0:
        P = sp.linalg.solve_continuous_are(a=sys.A, b=sys.B, q=Q, r=R)
        return np.linalg.inv(R) @ sys.B.T @ P
    else:
        P = sp.linalg.solve_discrete_are(a=sys.A, b=sys.B, q=Q, r=R)
        return np.linalg.inv(R + sys.B.T @ P @ sys.B) @ sys.B.T @ P @ sys.A
示例#6
0
 def _reduce_state_space(self):
     x_al    = self.fsf_dict['x_al']
     x_sp_al = self.fsf_dict['x_sp_al']
     x_er_al = self.fsf_dict['x_er_al']
     u_al    = self.fsf_dict['u_al']
     u_sp_al = self.fsf_dict['u_sp_al']
     u_er_al = self.fsf_dict['u_er_al']
     al_sys  = self.fsf_dict['al_sys']
     #Construct reduced state space
     x_ral    = x_al[7:]
     x_sp_ral = x_sp_al[7:]
     x_er_ral = x_er_al[7:]
     u_ral    = u_al[np.ix_([0,1,3,4])]
     u_sp_ral = u_sp_al[np.ix_([0,1,3,4])]
     u_er_ral = u_er_al[np.ix_([0,1,3,4])]
     A        = al_sys.A
     B        = al_sys.B
     C        = al_sys.C
     D        = al_sys.D
     A_ral    = A[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])]
     B_ral    = B[np.ix_([7,8,9,10,11,12],[0,1,3,4])]
     C_ral    = C[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])]
     D_ral    = D[np.ix_([7,8,9,10,11,12],[0,1,3,4])]
     #Lateral and longitudinal state spaces
     ral_sys = control.StateSpace(A_ral, B_ral, C_ral, D_ral)
     ral_ctrb = control.ctrb(ral_sys.A, ral_sys.B)
     ral_obsv = control.obsv(ral_sys.A, ral_sys.C)
     #Update FSF dictionary
     self.fsf_dict.update(x_ral = x_ral)
     self.fsf_dict.update(x_sp_ral = x_sp_ral)
     self.fsf_dict.update(x_er_ral = x_er_ral)
     self.fsf_dict.update(u_ral = u_ral)
     self.fsf_dict.update(u_sp_ral = u_sp_ral)
     self.fsf_dict.update(u_er_ral = u_er_ral)
     self.fsf_dict.update(ral_sys = ral_sys)
示例#7
0
    def test_point_to_point(self):
        # Machine precision for floats.
        eps = np.finfo(float).eps

        for states in range(1, self.maxStates):
            # Start with a random system
            linsys = matlab.rss(states, 1, 1)

            # Make sure the system is not degenerate
            Cmat = ctrl.ctrb(linsys.A, linsys.B)
            if (np.linalg.matrix_rank(Cmat) != states):
                if (self.debug):
                    print("  skipping (not reachable)")
                    continue

            if (self.debug): print(linsys)

            # Create a flat system representation
            flatsys = tg.LinearFlatSystem(linsys)

            # Generate several different initial and final conditions
            for i in range(self.numTests):
                x0 = np.random.rand(linsys.states)
                xf = np.random.rand(linsys.states)
                Tf = np.random.randn()

                # Generate a trajectory from start to stop
                systraj = tg.point_to_point(flatsys, x0, xf, Tf)
                xd, ud = systraj.eval((0, Tf))
                np.testing.assert_array_almost_equal(x0, xd[0, :], decimal=4)
                np.testing.assert_array_almost_equal(xf, xd[1, :], decimal=4)
    def test_point_to_point(self):
        # Machine precision for floats.
        eps = np.finfo(float).eps

        for states in range(1, self.maxStates):
            # Start with a random system
            linsys = matlab.rss(states, 1, 1)

            # Make sure the system is not degenerate
            Cmat = ctrl.ctrb(linsys.A, linsys.B)
            if (np.linalg.matrix_rank(Cmat) != states):
                if (self.debug):
                    print("  skipping (not reachable)")
                    continue

            if (self.debug): print(linsys)

            # Create a flat system representation
            flatsys = tg.LinearFlatSystem(linsys)

            # Generate several different initial and final conditions
            for i in range(self.numTests):
                x0 = np.random.rand(linsys.states)
                xf = np.random.rand(linsys.states)
                Tf = np.random.randn()

                # Generate a trajectory from start to stop
                systraj = tg.point_to_point(flatsys, x0, xf, Tf)
                xd, ud = systraj.eval((0,Tf))
                np.testing.assert_array_almost_equal(x0, xd[0,:], decimal=4)
                np.testing.assert_array_almost_equal(xf, xd[1,:], decimal=4)
示例#9
0
 def _check_if_controllable(self):
     C = ctrb(self.A, self.Bu)
     print('num. unstable = ', np.sum(np.abs(np.linalg.eig(self.A)[0]) > 1))
     rank = np.linalg.matrix_rank(C)
     # assert rank == self.A.shape[0]
     print('ctrb rank = ', rank)
     print('required rank = ', self.A.shape[0])
示例#10
0
def regulator_with_observer(sys,
                            poles=None,
                            po=None,
                            ts=None,
                            extra_poles=[],
                            ck=[]):
    final_poles = []

    if poles is not None:
        final_poles = poles

    if po is not None and ts is not None:
        log_po = np.log(100 / po)
        psi = log_po / np.sqrt(np.pi**2 + log_po**2)
        wn = 4 / psi / ts

        s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2)
        s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2)

        final_poles.append(s1)
        final_poles.append(s2)

    for p in extra_poles:
        final_poles.append(p)

    rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))

    total_poles = len(final_poles)

    if total_poles < rank:
        raise BaseException(
            f"you have {total_poles} but you need {rank} to implement a FSFB")

    k = ct.acker(sys.A, sys.B, final_poles)

    mo = ct.obsv(sys.A, sys.C)
    obs_rank = np.linalg.matrix_rank(mo)

    print("obs_rank=", obs_rank)

    final_poles_obs = 5 * final_poles

    L = ct.acker(sys.A.T, sys.C.T, final_poles_obs)
    L = L.T
    print("")
    print("L=")
    print(L)

    pa = np.concatenate([sys.A, -sys.B * k], axis=1)
    pb = np.concatenate([L * sys.C, sys.A - sys.B * k - L * sys.C], axis=1)

    alc = np.concatenate([pa, pb], axis=0)
    blc = np.concatenate(
        [np.zeros((sys.A.shape[0], 1)),
         np.zeros((sys.A.shape[0], 1))])
    clc = np.concatenate([sys.C, -sys.D * k], axis=1)
    dlc = np.array([[0]])

    return ct.ss(alc, blc, clc, dlc)
示例#11
0
def get_linear_quadratic_regulator(linear_model, q=numpy.identity(3),
                                   r=numpy.identity(1)):
    ctrb_matrix = control.ctrb(linear_model.A, linear_model.B)
    if ctrb_matrix.shape[0] != linear_model.A.shape[0]:
        raise ValueError("System is not controllable!")
    k_matrix, s_matrix, e_matrix = control.lqr(linear_model.A, linear_model.B,
                                               q, r)
    return k_matrix, s_matrix, e_matrix
示例#12
0
 def __isControllable(self, th=None):
     if th is None:
         th = self.__th
     F = np.array(self.__F(th))
     C = np.array(self.__C(th))
     n = self.__n
     ctrb_matrix = control.ctrb(F, C)
     rank = np.linalg.matrix_rank(ctrb_matrix)
     return rank == n
def full_observer(system, poles_o, poles_s, debug=False) -> ObserverResult:
    """implementation of the complete observer for an autonomous system
    :param system : tuple (a, b, c) of system matrices
    :param poles_o: tuple of complex poles/eigenvalues of the observer
    dynamics
    :param poles_s: tuple of complex poles/eigenvalues of the closed
    system
    :param debug: output control for debugging in unittest(False:normal
    output,True: output local variables and normal output)
    :return: dataclass ObserverResult (controller function, feedback matrix, observer_gain, and pre-filter)
    """

    # systemically relevant matrices
    a = system[0]
    b = system[1]
    c = system[2]
    d = system[3]

    # ignore the PendingDeprecationWarning for built-in packet control
    warnings.filterwarnings('ignore', category=PendingDeprecationWarning)

    ctr_matrix = ctr.ctrb(a, b)  # controllabilty matrix
    assert np.linalg.det(ctr_matrix) != 0, 'this system is not controllable'

    # State feedback
    f_t = ctr.acker(a, b, poles_s)

    # pre-filter
    a1 = a - b * f_t
    v = (-1 * (c * a1**(-1) * b)**(-1))  # pre-filter

    obs_matrix = ctr.obsv(a, c)  # observability matrix
    assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable'

    # calculate observer gain
    l_v = ctr.acker(a.T, c.T, poles_o).T

    # controller function
    # in this case controller function is: -1 * feedback * states + pre-filter * reference output
    # disturbance value ist not considered
    # t = sp.Symbol('t')
    # sys_input = -1 * (f_t * sys_state)[0]
    # input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy')

    # this method returns controller function, feedback matrix, observer gain and pre-filter
    result = ObserverResult(f_t, l_v, v, None)

    # return internal variables for unittest
    if debug:
        c_locals = Container(fetch_locals=True)
        result.debug = c_locals

    return result
示例#14
0
文件: Test1.py 项目: mgiglia92/RLBots
    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)
示例#15
0
    def design_controller_observer(self):
        if self.in_low_gear:
            q_vel = 1.0
        else:
            q_vel = 0.95

        q = [q_vel, q_vel]
        r = [12.0, 12.0]
        self.design_lqr(q, r)

        self.design_two_state_feedforward()

        q_vel = 1.0
        r_vel = 0.01
        self.design_kalman_filter([q_vel, q_vel], [r_vel, r_vel])

        print("ctrb cond =", np.linalg.cond(ct.ctrb(self.sysd.A, self.sysd.B)))
示例#16
0
    def feed_forward(self, A, B, C, D, K):
        """creates the feed forward matrix F to compute
        control inputs using a desired state"""

        F = np.linalg.pinv(-C * np.linalg.inv(A - B * K) * B)

        if np.linalg.matrix_rank(control.ctrb(A - B * K, B * F)) < A.shape[0]:
            print "System is not fully controllable"
        else:
            print "System is fully controllable"
        if np.linalg.matrix_rank(control.obsv(A - B * K,
                                              C - D * K)) < A.shape[0]:
            print "System is not fully observable"
        else:
            print "System is fully observable"

        return F
示例#17
0
文件: lqr.py 项目: mcm001/frccontrol
def lqr(*args, **kwargs):
    """Solves for the optimal linear-quadratic regulator (LQR).

    For a continuous system:

    .. math:: xdot = A * x + B * u
    .. math:: J = \int_0^\infty (x^T Q x + u^T R u + 2 x^T N u) dt

    For a discrete system:

    .. math:: x(n + 1) = A x(n) + B u(n)
    .. math:: J = \sum_0^\infty (x^T Q x + u^T R u + 2 x^T N u) \Delta T

    Keyword arguments:
    sys -- StateSpace object representing a linear system.
    Q -- numpy.array(states x states), state cost matrix.
    R -- numpy.array(inputs x inputs), control effort cost matrix.
    N -- numpy.array(states x inputs), cross weight matrix.

    Returns:
    K -- numpy.array(states x inputs), controller gain matrix.
    """
    sys = args[0]
    Q = args[1]
    R = args[2]
    if len(args) == 4:
        N = args[3]
    else:
        N = np.zeros((sys.A.shape[0], sys.B.shape[1]))

    m = sys.A.shape[0]

    controllability_rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
    if controllability_rank != m:
        print(
            "Warning: Controllability of %d != %d, uncontrollable state"
            % (controllability_rank, m)
        )

    if sys.dt == None:
        P = sp.linalg.solve_continuous_are(a=sys.A, b=sys.B, q=Q, r=R, s=N)
        return np.linalg.solve(R, sys.B.T @ P + N.T)
    else:
        P = sp.linalg.solve_discrete_are(a=sys.A, b=sys.B, q=Q, r=R, s=N)
        return np.linalg.solve(R + sys.B.T @ P @ sys.B, sys.B.T @ P @ sys.A + N.T)
示例#18
0
文件: Pendulum.py 项目: arolin/VeroPy
    def control_eq(self):
        self.equilibrium_point = hstack(( 0, pi / 2 * ones(len(self.q) - 1), zeros(len(self.u)) ))
        equilibrium_dict = dict(zip(self.q + self.u, self.equilibrium_point))
        parameter_dict = dict(zip(self.parameters, self.parameter_vals))

        # symbolically linearize about arbitrary equilibrium
        self.linear_state_matrix, self.linear_input_matrix, inputs = self.kane.linearize()
        # sub in the equilibrium point and the parameters
        self.f_A_lin = self.linear_state_matrix.subs(parameter_dict).subs(equilibrium_dict)
        self.f_B_lin = self.linear_input_matrix.subs(parameter_dict).subs(equilibrium_dict)
        m_mat = self.kane.mass_matrix_full.subs(parameter_dict).subs(equilibrium_dict)
        # compute A and B
        from numpy import matrix
        A = matrix(m_mat.inv() * self.f_A_lin)
        B = matrix(m_mat.inv() * self.f_B_lin)

        assert matrix_rank(control.ctrb(A, B)) == A.shape[0]

        self.K, self.X, self.E = control.lqr(A, B, ones(A.shape), 1);
示例#19
0
def regulator(sys, poles=None, po=None, ts=None, extra_poles=[], ck=[]):
    final_poles = []

    if poles is not None:
        final_poles = poles

    if po is not None and ts is not None:
        log_po = np.log(100 / po)
        psi = log_po / np.sqrt(np.pi**2 + log_po**2)
        wn = 4 / psi / ts

        s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2)
        s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2)

        final_poles.append(s1)
        final_poles.append(s2)

    for p in extra_poles:
        final_poles.append(p)

    rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))

    total_poles = len(final_poles)

    if total_poles < rank:
        raise BaseException(
            f"you have {total_poles} but you need {rank} to implement a FSFB")

    kk = ct.acker(sys.A, sys.B, final_poles)

    ak = sys.A - sys.B * kk
    bk = np.zeros((sys.A.shape[0], 1))
    default = np.zeros((1, sys.A.shape[0]))
    default[0] = 1
    ck2 = np.array(ck) if ck else default
    dk = np.array([[0]])

    return ct.ss(ak, bk, ck2, dk)
示例#20
0
    def __init__(self, *args, **params):
        # using dict.get(key, default) to set defaults
        self.b = params.get('motor_damping_constant', 1)
        self.k_motor = params.get('motor_force_constant', 1)
        self.m = params.get('chassis_mass', 1)
        self.width = params.get('track_width', 1)

        # calculate dynamics
        # x_dot = Ax + Bu
        # y     = Cx + Du
        b = self.b
        m = self.m
        w = self.width

        # A matrix
        self.A = np.matrix([[0, 1, 0, 0], [0, -b / m, 0, 0], [0, 0, 0, 1],
                            [0, 0, 0, -b * w / m]])

        # B matrix
        kdm = self.k_motor / m
        r = self.width / 2.0
        self.B = np.matrix([[0, 0], [kdm, kdm], [0, 0], [kdm * r, -kdm * r]])

        # C matrix
        self.C = np.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])

        # D matrix
        # D = 0
        self.D = np.zeros((self.C.shape[0], self.B.shape[1]))

        # reality check on feasibility of system
        self.ctrb = control.ctrb(self.A, self.B)
        assert int(np.linalg.matrix_rank(self.ctrb)) == int(
            self.ctrb.shape[0]), "System is not controllable!"

        #construct a continuous time system
        self.sys_ct = control.ss(self.A, self.B, self.C, self.D)
示例#21
0
# S**2 + alpha1*S + alpha0
th_alpha1 = 2.0 * th_zeta * th_wn
th_alpha0 = th_wn**2

# Desired Poles
des_char_poly_lat = np.convolve(
    np.convolve([1, s_alpha1, s_alpha0], [1, p_alpha1, p_alpha0]),
    np.poly(integrator_pole_lat))
des_char_poly_lon = np.convolve([1, th_alpha1, th_alpha0],
                                np.poly(integrator_pole_lon))
des_poles_lat = np.roots(des_char_poly_lat)
des_poles_lon = np.roots(des_char_poly_lon)

# Latitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A1_lat, B1_lat)) != 5:
    print("The Latitudinal system is not controllable")
else:
    K1_lat = cnt.acker(A1_lat, B1_lat, des_poles_lat)
    K_lat = K1_lat[0, 0:4]
    ki_lat = K1_lat[0, 4]

# Longitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A1_lon, B1_lon)) != 3:
    print("The Longitudinal system is not controllable")
else:
    K1_lon = cnt.acker(A1_lon, B1_lon, des_poles_lon)
    K_lon = K1_lon[0, 0:2]
    ki_lon = K1_lon[0, 2]

UNCERTAINTY_PARAMETERS = False
示例#22
0
    def testConvert(self):
        """Test state space to transfer function conversion."""
        verbose = self.debug
        from control.statesp import _mimo2siso
        
        #print __doc__

        # Machine precision for floats.
        eps = np.finfo(float).eps

        for states in range(1, self.maxStates):
            for inputs in range(1, self.maxIO):
                for outputs in range(1, self.maxIO):
                    # start with a random SS system and transform to TF then
                    # back to SS, check that the matrices are the same.
                    ssOriginal = matlab.rss(states, outputs, inputs)
                    if (verbose):
                        self.printSys(ssOriginal, 1)

                    # Make sure the system is not degenerate
                    Cmat = control.ctrb(ssOriginal.A, ssOriginal.B)
                    if (np.linalg.matrix_rank(Cmat) != states):
                        if (verbose):
                            print("  skipping (not reachable)")
                        continue
                    Omat = control.obsv(ssOriginal.A, ssOriginal.C)
                    if (np.linalg.matrix_rank(Omat) != states):
                        if (verbose):
                            print("  skipping (not observable)")
                        continue

                    tfOriginal = matlab.tf(ssOriginal)
                    if (verbose):
                        self.printSys(tfOriginal, 2)
                    
                    ssTransformed = matlab.ss(tfOriginal)
                    if (verbose):
                        self.printSys(ssTransformed, 3)

                    tfTransformed = matlab.tf(ssTransformed)
                    if (verbose):
                        self.printSys(tfTransformed, 4)

                    # Check to see if the state space systems have same dim
                    if (ssOriginal.states != ssTransformed.states):
                        print("WARNING: state space dimension mismatch: " + \
                            "%d versus %d" % \
                            (ssOriginal.states, ssTransformed.states))

                    # Now make sure the frequency responses match
                    # Since bode() only handles SISO, go through each I/O pair
                    # For phase, take sine and cosine to avoid +/- 360 offset
                    for inputNum in range(inputs):
                        for outputNum in range(outputs):
                            if (verbose):
                                print("Checking input %d, output %d" \
                                    % (inputNum, outputNum))
                            ssorig_mag, ssorig_phase, ssorig_omega = \
                                control.bode(_mimo2siso(ssOriginal, \
                                                        inputNum, outputNum), \
                                                 deg=False, Plot=False)
                            ssorig_real = ssorig_mag * np.cos(ssorig_phase)
                            ssorig_imag = ssorig_mag * np.sin(ssorig_phase)

                            #
                            # Make sure TF has same frequency response
                            #
                            num = tfOriginal.num[outputNum][inputNum]
                            den = tfOriginal.den[outputNum][inputNum]
                            tforig = control.tf(num, den)
                                                
                            tforig_mag, tforig_phase, tforig_omega = \
                                control.bode(tforig, ssorig_omega, \
                                                 deg=False, Plot=False)

                            tforig_real = tforig_mag * np.cos(tforig_phase)
                            tforig_imag = tforig_mag * np.sin(tforig_phase)
                            np.testing.assert_array_almost_equal( \
                                ssorig_real, tforig_real)
                            np.testing.assert_array_almost_equal( \
                                ssorig_imag, tforig_imag)

                            #
                            # Make sure xform'd SS has same frequency response
                            #
                            ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \
                                control.bode(_mimo2siso(ssTransformed, \
                                                        inputNum, outputNum), \
                                                 ssorig_omega, \
                                                 deg=False, Plot=False)
                            ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase)
                            ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase)
                            np.testing.assert_array_almost_equal( \
                                ssorig_real, ssxfrm_real)
                            np.testing.assert_array_almost_equal( \
                                ssorig_imag, ssxfrm_imag)

                            #
                            # Make sure xform'd TF has same frequency response
                            #
                            num = tfTransformed.num[outputNum][inputNum]
                            den = tfTransformed.den[outputNum][inputNum]
                            tfxfrm = control.tf(num, den)
                            tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
                                control.bode(tfxfrm, ssorig_omega, \
                                                 deg=False, Plot=False)
                            
                            tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase)
                            tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase)
                            np.testing.assert_array_almost_equal( \
                                ssorig_real, tfxfrm_real)
                            np.testing.assert_array_almost_equal( \
                                ssorig_imag, tfxfrm_imag)
示例#23
0
def check_controllability(A, B, n):
    Cmatrix = control.ctrb(A, B)
    rankCmatrix = np.linalg.matrix_rank(Cmatrix)
    print('controllability matrix rank is {}, ns={}, nz={}'.format(
        rankCmatrix, n, A.shape[0]))
    return rankCmatrix
示例#24
0
            ylim(0., 2.)
            grid()
            if ii == 0:
                title(
                    'Predicted state evolution of different models with open loop control'
                )
        xlabel('Time (sec)')
        legend(fontsize=10, loc='best')
        savefig(open_filename, format='pdf', dpi=2400)
        show()
        #close()
    print('in {:.2f}s'.format(time.process_time() - t0))

    print('in {:.2f}s'.format(time.process_time() - t0))

Cmatrix = control.ctrb(A=edmd_model.A, B=edmd_model.B)
print('edmd controllability matrix rank is {}, ns={}, nz={}'.format(
    np.linalg.matrix_rank(Cmatrix), n, edmd_model.A.shape[0]))
print(n_lift_edmd)

#%%
#!==============================================  EVALUATE PERFORMANCE -- CLOSED LOOP =============================================
t0 = time.process_time()
print('Evaluate Performance with closed loop trajectory tracking...', end=" ")
# Set up trajectory and controller for prediction task:
x_0 = array([2., 0.25, 0., 0.])
mpc_controller.eval(x_0, 0)
q_d_pred = mpc_controller.parse_result()

x_0 = q_d_pred[:, 0]
t_pred = t_d.squeeze()
示例#25
0
Cout = np.array([[0.0, 1.0, 0.0, 0.0]])
A1 = np.array([[0.0, 0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0],
               [-P.k / P.Js, P.k / P.Js, -P.b / P.Js, P.b / P.Js, 0.0],
               [P.k / P.Jp, -P.k / P.Jp, P.b / P.Jp, -P.b / P.Jp, 0.0],
               [0.0, -1.0, 0.0, 0.0, 0.0]])

B1 = np.array([[0.0], [0.0], [1.0 / P.Js], [0.0], [0.0]])

# gain calculation
des_char_poly = np.convolve(
    np.convolve([1, 2 * zeta_phi * wn_phi, wn_phi**2],
                [1, 2 * zeta_th * wn_th, wn_th**2]), np.poly(integrator_pole))
des_poles = np.roots(des_char_poly)

# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 5:
    print("The system is not controllable")
else:
    K1 = cnt.acker(A1, B1, des_poles)
    K = np.matrix([K1.item(0), K1.item(1), K1.item(2), K1.item(3)])
    ki = K1.item(4)

# computer observer gains
des_obs_char_poly = np.convolve([1, 2 * zeta_phi * wn_phi_obs, wn_phi_obs**2],
                                [1, 2 * zeta_th * wn_th_obs, wn_th_obs**2])
des_obs_poles = np.roots(des_obs_char_poly)

# Compute the gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 4:
    print("The system is not observable")
else:
示例#26
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()
示例#27
0
Q = np.eye(4,4)
R = np.eye(2,2)

#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)

S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)

print "ctrb = "+ str(np.linalg.matrix_rank(ct.ctrb(Ap,Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap,Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap,Bp,Cp,Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P,t)
#plt.plot(t,stepy)
#ct.impulse(ss_P)
ct.nyquist(ss_P)
#bode plot only implemented for SISO system.omg
#ct.bode(ss_P)
示例#28
0
def dare(F, G1, G2, H):
    """Solves the discrete-time algebraic Riccati equation

    0 = F ^ T * X * F
        - X - F ^ T * X * G1 * (G2 + G1 ^ T * X * G1) ^ -1 * G1 ^ T * X * F + H

    Under the assumption that X ^ -1 exists, this equation is equivalent to

    0 = F ^ T * (X ^ -1 + G1 * G2 ^ -1 * G1 ^ T) ^ -1 * F - X + H

    Parameters
    ==========
    Inputs are real matrices:

    F : n x n
    G1 : n x m
    G2 : m x m, symmetric, positive definite
    H : n x n, symmetric, positive semi-definite

    Assumptions
    ===========
    (F, G1) is a stabilizable pair
    (C, F) is a detectable pair (where C is full rank factorization of H, i.e.,
        C ^ T * C = H and rank(C) = rank(H).
    F is invertible

    Returns
    =======

    Unique nonnegative definite solution of discrete Algrebraic Ricatti
    equation.

    Notes
    =====
    This is an implementation of the Schur method for solving algebraic Riccati
    eqautions as described in dx.doi.org/10.1109/TAC.1979.1102178

    """
    # Verify that F is non-singular
    u, s, v = la.svd(F)
    assert(np.all(s > 0.0))
    # Verify that (F, G1) controllable
    C = ctrb(F, G1)
    u, s, v = la.svd(C)
    assert(np.all(s > 0.0))
    # Verify that (H**.5, F) is observable
    O = obsv(H**.5, F)
    u, s, v = la.svd(O)
    assert(np.all(s > 0.0))
    
    n = F.shape[0]
    m = G2.shape[0]

    G = np.dot(G1, np.dot(inv(G2), G1.T))
    Finv = inv(F)
    Finvt = Finv.T

    # Form symplectic matrix
    Z = empty((2*n, 2*n))
    Z[:n, :n] = F + np.dot(G, np.dot(Finvt, H))
    Z[:n, n:] = -np.dot(G, Finvt)
    Z[n:, :n] = -np.dot(Finvt, H)
    Z[n:, n:] = Finvt

    S, U, sdim = schur(Z, sort='iuc')

    # Verify that the n eigenvalues of the upper left block stable
    assert(sdim == n)

    U11 = U[:n, :n]
    U21 = U[n:, :n]
    return solve(U[:n, :n].T, U[n:, :n].T).T
# xdot = A*x + B*u
# y = C*x
A = np.array([[0.0, 1.0], [-P.k / P.m, -P.b / P.m]])

B = np.array([[0.0], [1 / P.m]])

C = np.array([[1.0, 0.0]])

# form augmented system
A1 = np.array([[0.0, 1.0, 0.0], [-P.k / P.m, -P.b / P.m, 0.0],
               [-1.0, 0.0, 0.0]])

B1 = np.array([[0.0], [1 / P.m], [0.0]])

# gain calculation
wn = 2.2 / tr  # natural frequency
des_char_poly = np.convolve([1, 2 * zeta * wn, wn**2],
                            np.poly(integrator_pole))
des_poles = np.roots(des_char_poly)

# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
    print("The system is not controllable")
else:
    K1 = cnt.acker(A1, B1, des_poles)
    K = np.array([[K1.item(0), K1.item(1)]])
    ki = K1.item(2)

print('K: ', K)
print('ki: ', ki)
示例#30
0
B = np.matrix([[0], [1 / (P.mc + 2 * P.mr)]])

C = np.matrix([[1.0, 0.0]])

# Augmented Longitudinal Parameters
A1 = np.matrix([[0.0, 1.0, 0.0], [0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]])

B1 = np.matrix([[0], [1 / (P.mc + 2 * P.mr)], [0.0]])

# gain calculation
des_char_poly = np.convolve([1, 2 * zeta_h * wn_h, wn_h**2],
                            np.poly([h_integrator_pole]))
des_poles = np.roots(des_char_poly)

# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
    print("The longitudinal system is not controllable")
else:
    K1 = cnt.acker(A1, B1, des_poles)
    K = np.matrix([K1.item(0), K1.item(1)])
    ki = K1.item(2)

# ----------------------------------------- OBSERVER GAINS -----------------------------------------
# compute longitudinal observer gains
des_obs_char_poly = [1, 2 * zeta_h * wn_h_obs, wn_h_obs**2]
des_obs_poles = np.roots(des_obs_char_poly)

# Compute the gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
    print("The longitudinal observer system is not observable")
else:
示例#31
0
def train_net(u_all_training,y_all_training,mean_diff_nocovar,optimizer,u_control_all_training=None,valid_error_thres=1e-2,test_error_thres=1e-2,max_iters=100000,step_size_val=0.01):
  iter = 0;
  samplerate = 5000;
  good_start = 1;
  valid_error = 100.0;
  test_error = 100.0;
  training_error_history_nocovar = [];
  validation_error_history_nocovar = [];
  test_error_history_nocovar = [];

  training_error_history_withcovar = [];
  validation_error_history_withcovar = [];
  test_error_history_withcovar = [];

  covar_actual = compute_covarmat(u_all_training,y_all_training);
  covar_model_history = [];
  covar_diff_history = [];
  while (((test_error>test_error_thres) or (valid_error > valid_error_thres)) and iter < max_iters):
    iter+=1;
    
    all_ind = set(np.arange(0,len(u_all_training)));
    select_ind = np.random.randint(0,len(u_all_training),size=batchsize);
    valid_ind = list(all_ind -set(select_ind))[0:batchsize];
    select_ind_test = list(all_ind - set(valid_ind) - set(select_ind))[0:batchsize];

    
    u_batch =[];
    u_control_batch = [];
    y_batch = [];
    u_valid = [];
    u_control_valid = [];
    y_valid = [];
    u_test_train = [];
    u_control_train = [];
    y_test_train= [];
    u_control_test_train = [];
    
    for j in range(0,len(select_ind)):
      u_batch.append(u_all_training[select_ind[j]]);
      y_batch.append(y_all_training[select_ind[j]]);
      if with_control:
          u_control_batch.append(u_control_all_training[select_ind[j]]);
          
    for k in range(0,len(valid_ind)):
      u_valid.append(u_all_training[valid_ind[k]]);
      y_valid.append(y_all_training[valid_ind[k]]);
      if with_control:
          u_control_valid.append(u_control_all_training[valid_ind[k]]);

    for k in range(0,len(select_ind_test)):
      u_test_train.append(u_all_training[select_ind_test[k]]);
      y_test_train.append(y_all_training[select_ind_test[k]]);
      if with_control:
          u_control_test_train.append(u_control_all_training[select_ind_test[k]]);


    if with_control:
      optimizer.run(feed_dict={yp_feed:u_batch,yf_feed:y_batch,u_control:u_control_batch,step_size:step_size_val});
      valid_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid,u_control:u_control_valid});
      test_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train,u_control:u_control_test_train});

    else:
      optimizer.run(feed_dict={yp_feed:u_batch,yf_feed:y_batch,step_size:step_size_val});
      valid_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid});
      test_error = mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train});


    
    if iter%samplerate==0:
      if with_control:
        training_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_batch,yf_feed:y_batch,u_control:u_control_batch}));
        validation_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid,u_control:u_control_valid}));
        test_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train,u_control:u_control_test_train}));
      else:
        training_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_batch,yf_feed:y_batch}));
        validation_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid}));
        test_error_history_nocovar.append(mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train}));
      
  
      if (iter%10==0) or (iter==1):
        plt.close();
        if plot_deep_basis:
          fig_hand = expose_deep_basis(psiypz_list,num_bas_obs,deep_dict_size,iter,yp_feed);
          fig_hand = quick_nstep_predict(Y_p_old,u_control_all_training,with_control,num_bas_obs,iter);

        if with_control:  
          print ("step %d , validation error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid,u_control:u_control_valid})));
          print ("step %d , test error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train,u_control:u_control_test_train})));

            #print ( test_synthesis(sess.run(Kx).T,sess.run(Ku).T ))
        else:
          print ("step %d , validation error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_valid,yf_feed:y_valid})));
          print ("step %d , test error %g"%(iter, mean_diff_nocovar.eval(feed_dict={yp_feed:u_test_train,yf_feed:y_test_train})));
          
    if ((iter>20000) and iter%10) :

      valid_gradient = np.gradient(np.asarray(validation_error_history_nocovar[np.int(iter/samplerate*3/10):]) );
      mu_gradient = np.mean(valid_gradient);

      if ((iter <1000) and (mu_gradient >= 5e-1)): # eventually update this to be 1/10th the mean of batch data, or mean of all data handed as input param to func
        good_start = 0; # if after 10,000 iterations validation error is still above 1e0, initialization was poor.
        print("Terminating model refinement loop with gradient:") + repr(mu_gradient) + ", validation error after " + repr(iter) + " epochs:  " + repr(valid_error);
        iter = max_iters; # terminate while loop and return histories


      
    if iter > 10000 and with_control:
      At = sess.run(Kx).T;
      Bt = sess.run(Ku).T;
      ctrb_rank = np.linalg.matrix_rank(control.ctrb(At,Bt));
      if debug_splash:
        print(repr(ctrb_rank) + " : " + repr(control.ctrb(At,Bt).shape[0]));
      
      if ctrb_rank == control.ctrb(At,Bt).shape[0] and test_error_history_nocovar[-1] <1e-5:
         iter=max_iters;
         

        

  all_histories = [training_error_history_nocovar, validation_error_history_nocovar,test_error_history_nocovar, \
                   training_error_history_withcovar,validation_error_history_withcovar,test_error_history_withcovar,covar_actual,covar_diff_history,covar_model_history];
    
  
  plt.close();
  x = np.arange(0,len(validation_error_history_nocovar),1);
  plt.plot(x,training_error_history_nocovar,label='train. err.');
  plt.plot(x,validation_error_history_nocovar,label='valid. err.');
  plt.plot(x,test_error_history_nocovar,label='test err.');
  #plt.gca().set_yscale('log');
  plt.savefig('all_error_history.pdf');

  plt.close();
  return all_histories,good_start;
示例#32
0
def main():
    r = 0.05
    L = 0.235
    w_ref_l = 3
    w_ref_r = w_ref_l
    v_bar = r / 2 * (w_ref_l + w_ref_r)

    A = np.array([[v_bar, -r / L, r / L], [0, -0.1, 0], [0, 0, -0.1]])

    print(np.linalg.inv(A))
    B = np.array([[0, 0], [1, 0], [0, 1]])

    Q = control.ctrb(A, B)  # reachability matrix

    C = np.eye(3)
    D = np.zeros((3, 2))

    # F, G, H, M, dt = sig.cont2discrete((A, B, C, D), .01)
    # print(F)

    Q = np.array([[100, 0, 0], [0, 1, 0], [0, 0, 1]])
    r_lqr = 0.01
    R = r_lqr * np.eye(2)

    K, S, E = controlpy.synthesis.controller_lqr(A, B, Q, R)
    # K, S, E = controlpy.synthesis.controller_lqr(F, G, Q, R)
    print(K)

    state_space = control.StateSpace(A - B @ K, B, C, D)
    # state_space = control.StateSpace(F - G @ K, G, H, M)

    Z = C @ np.linalg.inv(-A + B @ K) @ B  # Intermediate term
    # Z = H @ np.linalg.inv(-F + G @ K) @ G  # Intermediate term
    F = np.linalg.inv(Z.T @ Z) @ Z.T

    ref = np.array([[0], [3], [3]])
    Fr = F @ ref
    t = np.arange(0, 6, .01)
    Fr_t = Fr * np.ones((2, len(t)))

    T, y_out, x_out = control.forced_response(state_space,
                                              t,
                                              Fr_t,
                                              X0=[.2, 3, 3])  # .2, 3, 3
    fig1, ax1 = plt.subplots()
    ax1.plot(T, y_out[0], T, y_out[1], T, y_out[2])

    T, y_out, x_out = control.forced_response(state_space,
                                              t,
                                              Fr_t,
                                              X0=[0, 0, 0])  # .2, 3, 3
    fig2, ax2 = plt.subplots()
    ax2.plot(T, y_out[0], T, y_out[1], T, y_out[2])

    T, y_out, x_out = control.forced_response(state_space,
                                              t,
                                              Fr_t,
                                              X0=[-.2, 3, 3])  # .2, 3, 3
    fig3, ax3 = plt.subplots()
    ax3.plot(T, y_out[0], T, y_out[1], T, y_out[2])

    plt.show()
示例#33
0
#  tuning parameters
tr = 0.4
zeta = 0.707

# State Space Equations
# xdot = A*x + B*u
# y = C*x
A = np.array([[0.0, 1.0], [0.0, -1.0 * P.b / P.m / (P.ell**2)]])

B = np.array([[0.0], [3.0 / P.m / (P.ell**2)]])

C = np.array([[1.0, 0.0]])

# gain calculation
wn = 2.2 / tr  # natural frequency
des_char_poly = [1, 2 * zeta * wn, wn**2]
des_poles = np.roots(des_char_poly)

# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 2:
    print("The system is not controllable")

else:
    #.A just turns K matrix into a numpy array
    K = (cnt.acker(A, B, des_poles)).A
    kr = -1.0 / (C @ np.linalg.inv(A - B @ K) @ B)

print('K: ', K)
print('kr: ', kr)
示例#34
0
# MÉTODO 2 xa[:, k] = np.array([[dx[:, k]],
# [  y[k]  ]])

Aa = np.array([[A[0, 0], A[0, 1], 0], [A[1, 0], A[1, 1], 0],
               [np.dot(C, A)[0], np.dot(C, A)[1], 1]])

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

Ca = np.array([0, 0, 1])
Da = 0

# %% Controllability and Observability analysis

Co = control.ctrb(Aa, Ba)
if matrix_rank(Co) == len(Aa):
    print('The augmented system is controllable.')

Ob = control.obsv(Aa, Ca)
if matrix_rank(Ob) == len(Aa):
    print('The augmented system is observable.')

# %% LQR controller

#               y | Delta*x1 | Delta*x2
Qlqr = np.diag([1, 100, 1])
Rlqr = 1 * np.eye(2)
# K, X, eigVals = dlqr(Aa, Ba, Qlqr, Rlqr)

# Recursive Riccati Difference Equation (RDE)
示例#35
0
C_h = np.array([1.0, 0.0])

# gain calculation
wn_th = 2.2 / tr_theta  # natural frequency for angle
wn_z = 2.2 / tr_z  # natural frequency for latral
wn_h = 2.2 / tr_h  # natural frequency for longitudinal

#-----------------------------latral-----------------------------------------

des_char_poly_z = np.convolve([1, 2 * zeta_z * wn_z, wn_z**2],
                              [1, 2 * zeta_th * wn_th, wn_th**2])
des_poles_z = np.roots(des_char_poly_z)

# Compute the gains if the system is controllable for latral
if np.linalg.matrix_rank(cnt.ctrb(A_z, B_z)) != 4:
    print("The system is not controllable")
else:
    K_z = cnt.acker(A_z, B_z, des_poles_z)
    Cr_z = np.array([[1.0, 0.0, 0.0, 0.0]])
    kr_z = -1.0 / (Cr_z * np.linalg.inv(A_z - B_z @ K_z) @ B_z)

print('K_z: ', K_z)
print('kr_z: ', kr_z)

#----------------------------longi----------------------------------------
des_char_poly_h = [1, 2 * zeta_h * wn_h, wn_h**2]
des_poles_h = np.roots(des_char_poly_h)

# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A_h, B_h)) != 2:
示例#36
0
    def sparse(self, gamma, niter):

        zero_thres = 1.e-6

        print("gamma = %e | number of modes =         " % (gamma))

        # Define and solve the sparsity-promoting optimization problem
        # Weighted L1 norm is updated iteratively
        alpha = cp.Variable(self.q)
        weights = np.ones(self.q)
        for i in range(niter):
            objective_sparse = cp.Minimize(
                cp.quad_form(alpha, self.P) -
                2. * cp.real(self.d.conj().T @ alpha) + self.s +
                gamma * cp.pnorm(np.diag(weights) @ alpha, p=1))
            prob_sparse = cp.Problem(objective_sparse)
            sol_sparse = prob_sparse.solve(verbose=False, solver=cp.SCS)

            alpha_sp = alpha.value  # Sparse solution
            if alpha_sp is None:
                alpha_sp = np.ones(self.q)

            # Update weights
            weights = 1.0 / (np.abs(alpha_sp) + np.finfo(float).eps)

            nonzero = np.abs(alpha_sp) > zero_thres  # Nonzero modes
            print("                               %d of %d" %
                  (np.sum(nonzero), self.q))

        J_sp = np.real(alpha_sp.conj().T @ self.P @ alpha_sp -
                       2 * np.real(self.d.conj().T @ alpha_sp) +
                       self.s)  # Square error
        nx = np.sum(
            nonzero
        )  # Number of nonzero modes - order of the sparse/reduced model

        Ez = np.eye(self.q)[:, ~nonzero]

        # Define and solve the refinement optimization problem
        #alpha = cp.Variable(self.q)
        objective_refine = cp.Minimize(
            cp.quad_form(alpha, self.P) -
            2. * cp.real(self.d.conj().T @ alpha) + self.s)
        if np.sum(~nonzero):
            constraint_refine = [Ez.T @ alpha == 0]
            prob_refine = cp.Problem(objective_refine, constraint_refine)
        else:
            prob_refine = cp.Problem(objective_refine)

        sol_refine = prob_refine.solve()

        alpha_ref = alpha.value
        J_ref = np.real(alpha_ref.conj().T @ self.P @ alpha_ref -
                        2 * np.real(self.d.conj().T @ alpha_ref) +
                        self.s)  # Square error

        P_loss = 100 * np.sqrt(J_ref / self.s)

        # Truncate modes
        E = np.eye(self.q)[:, nonzero]
        Lambda_bar = E.T @ self.Lambda @ E
        Beta_bar = E.T @ self.Gamma
        Phi_bar = self.Phi @ np.diag(alpha_ref) @ E

        # Save data
        stats = {}
        stats["nx"] = nx
        stats["alpha_sp"] = alpha_sp
        stats["alpha_ref"] = alpha_ref
        stats["z_0"] = (np.linalg.pinv(self.Phi) @ self.Y0[:, 0])[nonzero]
        stats["E"] = E
        stats["J_sp"] = J_sp
        stats["J_ref"] = J_ref
        stats["P_loss"] = P_loss

        if nx != 0:
            print("Rank of controllability matrix: %d of %d" %
                  (np.linalg.matrix_rank(control.ctrb(Lambda_bar,
                                                      Beta_bar)), nx))

        # Convert system from complex modal form to real block-modal form
        lambda_bar = np.diag(Lambda_bar)
        A = np.zeros((nx, nx))
        B = np.zeros((nx, self.nu))
        Theta = np.zeros((self.ny, nx))

        i = 0
        while i < nx:
            if np.isreal(lambda_bar[i]):
                A[i, i] = lambda_bar[i]
                B[i] = Beta_bar[i]
                Theta[:, i] = Phi_bar[:, i]
                i += 1
            elif i == nx - 1:
                # In case only one of the conjugate pairs is truncated - this rarely happens
                A[i, i] = np.real(lambda_bar[i])
                B[i] = np.real(Beta_bar[i])
                Theta[:, i] = np.real(Phi_bar[:, i])
                i += 1
            elif np.isreal(np.isreal(lambda_bar[i] + lambda_bar[i + 1])):
                A[i:i + 2, i:i + 2] = np.array(
                    [[np.real(lambda_bar[i]),
                      np.imag(lambda_bar[i])],
                     [-np.imag(lambda_bar[i]),
                      np.real(lambda_bar[i])]])
                B[i] = np.real(Beta_bar[i])
                B[i + 1] = -np.imag(Beta_bar[i])
                Theta[:, i] = 2 * np.real(Phi_bar[:, i])
                Theta[:, i + 1] = 2 * np.imag(Phi_bar[:, i])
                i += 2
            else:
                raise ValueError(
                    "Eigenvalues are not grouped in conjugate pairs")

    #return StateSpace(Lambda_bar, Beta_bar, Phi_bar), lambda_bar, stats
        return StateSpace(A, B, Theta), lambda_bar, stats