How to use control - 10 common examples

def firstorder_gen(params, period=0.01, noiseamp=1.0):
    """Simple first order transfer function affected variable
    with sinusoid cause.

    """

    samples = params[0]
    delay = params[1]

    P1 = control.matlab.tf([10], [100, 1])

    timepoints = np.array(range(samples + delay))

    sine_input = np.array([np.sin(period * t * 2 * np.pi) for t in timepoints])

    P1_response = control.matlab.lsim(P1, sine_input, timepoints)

    affected_random_add = (seed_rand(51, samples + delay) - 0.5) * noiseamp

    cause = sine_input[:samples]

    if delay == 0:
        offset = None
    else:
        offset = samples

    affected = P1_response[0][delay:] + affected_random_add[delay:]

    tspan = P1_response[1][:offset]

    return tspan, cause, affected

[0., 1., 0., 0.]])
    B = np.array([[1./tau, 0.],
                  [0., 1./tau],
                  [0., 0.],
                  [0., 0.]])

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

    # make simulator with coninuous matrix
    init_xs = np.array([0., 0., 0., 0.])
    plant_cvxopt = FirstOrderSystem(A, B, C, init_states=init_xs)
    plant_scipy = FirstOrderSystem(A, B, C, init_states=init_xs)

    # create system
    sysc = matlab.ss(A, B, C, D)
    # discrete system
    sysd = matlab.c2d(sysc, dt)

    Ad = sysd.A
    Bd = sysd.B

    # evaluation function weight
    Q = np.diag([1., 1., 10., 10.])
    R = np.diag([0.01, 0.01])
    pre_step = 5

    # make controller with discreted matrix
    # please check the solver, if you want to use the scipy, set the MpcController_scipy
    controller_cvxopt = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
                               dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
                               input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.]))

legend(loc='best')

        #Test with matrices
        subplot2grid(plot_shape, (1, 0))
        t = matrix(linspace(0, 1, 100))
        u = matrix(r_[1:1:50j, 0:0:50j])
        x0 = matrix("0.; 0")
        y, t_out, _x = lsim(sys, u, t, x0)
        plot(t_out, y, label='y')
        plot(t_out, asarray(u/10)[0], label='u/10')
        legend(loc='best')

        #Test with MIMO system
        subplot2grid(plot_shape, (1, 1))
        A, B, C, D = self.make_MIMO_mats()
        sys = ss(A, B, C, D)
        t = matrix(linspace(0, 1, 100))
        u = array([r_[1:1:50j, 0:0:50j],
                   r_[0:1:50j, 0:0:50j]])
        x0 = [0, 0, 0, 0]
        y, t_out, _x = lsim(sys, u, t, x0)
        plot(t_out, y[0], label='y[0]')
        plot(t_out, y[1], label='y[1]')
        plot(t_out, u[0]/10, label='u[0]/10')
        plot(t_out, u[1]/10, label='u[1]/10')
        legend(loc='best')


        #Test with wrong values for t
        #T is None; - special handling: Value error
        self.assertRaises(ValueError, lsim(sys, U=0, T=None, x0=0))
        #T="hello" : Wrong type

def test_step(self):
        """Test function ``step``."""
        figure(); plot_shape = (1, 3)

        #Test SISO system
        A, B, C, D = self.make_SISO_mats()
        sys = ss(A, B, C, D)
        #print sys
        #print "gain:", dcgain(sys)

        subplot2grid(plot_shape, (0, 0))
        t, y = step(sys)
        plot(t, y)

        subplot2grid(plot_shape, (0, 1))
        T = linspace(0, 2, 100)
        X0 = array([1, 1])
        t, y = step(sys, T, X0)
        plot(t, y)

        #Test MIMO system
        A, B, C, D = self.make_MIMO_mats()
        sys = ss(A, B, C, D)

#print sys_siso_11

        #gain of converted system and equivalent SISO system must be the same
        self.assert_systems_behave_equal(sys_siso, sys_siso_00)
        self.assert_systems_behave_equal(sys_siso, sys_siso_11)

        #Test with additional systems --------------------------------------------
        #They have crossed inputs and direct feedthrough 
        #SISO system
        As = matrix([[-81.82, -45.45],
                     [ 10.,    -1.  ]])
        Bs = matrix([[9.09],
                     [0.  ]])
        Cs = matrix([[0, 0.159]])
        Ds = matrix([[0.02]])
        sys_siso = ss(As, Bs, Cs, Ds)
        #    t, y = step(sys_siso)
        #    plot(t, y, label='sys_siso d=0.02')
        #    legend(loc='best')

        #MIMO system
        #The upper left sub-system uses : input 0, output 1
        #The lower right sub-system uses: input 1, output 0
        Am = array([[-81.82, -45.45,   0,      0   ],
                    [ 10,     -1,      0,      0   ],
                    [  0,      0,    -81.82, -45.45],
                    [  0,      0,     10,     -1,  ]])
        Bm = array([[9.09, 0   ],
                    [0   , 0   ],
                    [0   , 9.09],
                    [0   , 0   ]])
        Cm = array([[0, 0,     0, 0.159],

figure()

        #everything automatically
        t, y = impulse(sys)
        plot(t, y, label='Simple Case')

        #supply time and X0
        T = linspace(0, 2, 100)
        X0 = [0.2, 0.2]
        t, y = impulse(sys, T, X0)
        plot(t, y, label='t=0..2, X0=[0.2, 0.2]')

        #Test system with direct feed-though, the function should print a warning.
        D = [[0.5]]
        sys_ft = ss(A, B, C, D)
        t, y = impulse(sys_ft)
        plot(t, y, label='Direct feedthrough D=[[0.5]]')

        #Test MIMO system
        A, B, C, D = self.make_MIMO_mats()
        sys = ss(A, B, C, D)
        t, y = impulse(sys)
        plot(t, y, label='MIMO System')

        legend(loc='best')
        #show()

    @unittest.skip("skipping test_impulse, need to update test")
    def test_impulse(self):
        A, B, C, D = self.make_SISO_mats()
        sys = ss(A, B, C, D)

        figure()

        #everything automatically
        t, y = impulse(sys)
        plot(t, y, label='Simple Case')

        #supply time and X0
        T = linspace(0, 2, 100)
        X0 = [0.2, 0.2]
        t, y = impulse(sys, T, X0)
        plot(t, y, label='t=0..2, X0=[0.2, 0.2]')

        #Test system with direct feed-though, the function should print a warning.
        D = [[0.5]]
        sys_ft = ss(A, B, C, D)

def test_dcgain_2(self):
        """Test function dcgain with different systems"""
        #Create different forms of a SISO system
        A, B, C, D = self.make_SISO_mats()
        Z, P, k = scipy.signal.ss2zpk(A, B, C, D)
        num, den = scipy.signal.ss2tf(A, B, C, D)
        sys_ss = ss(A, B, C, D)

        #Compute the gain with ``dcgain``
        gain_abcd = dcgain(A, B, C, D)
        gain_zpk = dcgain(Z, P, k)
        gain_numden = dcgain(np.squeeze(num), den)
        gain_sys_ss = dcgain(sys_ss)
        print 'gain_abcd:', gain_abcd, 'gain_zpk:', gain_zpk
        print 'gain_numden:', gain_numden, 'gain_sys_ss:', gain_sys_ss

        #Compute the gain with a long simulation
        t = linspace(0, 1000, 1000)
        y, _t = step(sys_ss, t)
        gain_sim = y[-1]
        print 'gain_sim:', gain_sim

        #All gain values must be approximately equal to the known gain

def assert_systems_behave_equal(self, sys1, sys2):
        '''
        Test if the behavior of two Lti systems is equal. Raises ``AssertionError``
        if the systems are not equal.

        Works only for SISO systems.

        Currently computes dcgain, and computes step response.
        '''
        #gain of both systems must be the same
        assert_array_almost_equal(dcgain(sys1), dcgain(sys2))

        #Results of ``step`` simulation must be the same too
        t, y1 = step(sys1)
        _t, y2 = step(sys2, t)
        assert_array_almost_equal(y1, y2)

sys = frdata.FRD(sysdata, smooth=True)
        elif isinstance(sysdata, xferfcn.TransferFunction):
            sys = sysdata
        elif getattr(sysdata, '__iter__', False) and len(sysdata) == 3:
            mag, phase, omega = sysdata
            sys = frdata.FRD(mag * np.exp(1j * phase * math.pi/180),
                             omega, smooth=True)
        else:
            sys = xferfcn._convert_to_transfer_function(sysdata)
    except Exception as e:
        print (e)
        raise ValueError("Margin sysdata must be either a linear system or "
                         "a 3-sequence of mag, phase, omega.")

    # calculate gain of system
    if isinstance(sys, xferfcn.TransferFunction):

        # check for siso
        if not issiso(sys):
            raise ValueError("Can only do margins for SISO system")

        # real and imaginary part polynomials in omega:
        rnum, inum = _polyimsplit(sys.num[0][0])
        rden, iden = _polyimsplit(sys.den[0][0])

        # test (imaginary part of tf) == 0, for phase crossover/gain margins
        test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden))
        w_180 = np.roots(test_w_180)

        # first remove imaginary and negative frequencies, epsw removes the
        # "0" frequency for type-2 systems
        w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 >= epsw)])