示例#1
0
def reachable_form(xsys):
    """Convert a system into reachable canonical form
    Parameters
    ----------
    xsys : StateSpace object
        System to be transformed, with state `x`
    Returns
    -------
    zsys : StateSpace object
        System in reachable canonical form, with state `z`
    T : matrix
        Coordinate transformation: z = T * x
    """
    # Check to make sure we have a SISO system
    if not issiso(xsys):
        raise ControlNotImplemented(
            "Canonical forms for MIMO systems not yet supported")

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

    # Generate the system matrices for the desired canonical form
    zsys.B = zeros(shape(xsys.B))
    zsys.B[0, 0] = 1.0
    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.0

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

    if matrix_rank(Wrx) != xsys.states:
        raise ValueError("System not controllable to working precision.")

    # Transformation from one form to another
    Tzx = solve(Wrx.T, Wrz.T).T  # matrix right division, Tzx = Wrz * inv(Wrx)

    if matrix_rank(Tzx) != xsys.states:
        raise ValueError("Transformation matrix singular to working precision.")

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

    return zsys, Tzx
示例#2
0
    def testAcker(self, fixedseed):
        for states in range(1, self.maxStates):
            for i in range(self.maxTries):
                # start with a random SS system and transform to TF then
                # back to SS, check that the matrices are the same.
                sys = rss(states, 1, 1)
                if (self.debug):
                    print(sys)

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

                # Place the poles at random locations
                des = rss(states, 1, 1)
                poles = pole(des)

                # Now place the poles using acker
                K = acker(sys.A, sys.B, poles)
                new = ss(sys.A - sys.B * K, sys.B, sys.C, sys.D)
                placed = pole(new)

                # Debugging code
                # diff = np.sort(poles) - np.sort(placed)
                # if not all(diff < 0.001):
                #     print("Found a problem:")
                #     print(sys)
                #     print("desired = ", poles)

                np.testing.assert_array_almost_equal(np.sort(poles),
                                                     np.sort(placed),
                                                     decimal=4)
示例#3
0
    def testAcker(self):
        for states in range(1, self.maxStates):
            for i in range(self.maxTries):
                # start with a random SS system and transform to TF then
                # back to SS, check that the matrices are the same.
                sys = rss(states, 1, 1)
                if (self.debug):
                    print(sys)

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

                # Place the poles at random locations
                des = rss(states, 1, 1);
                poles = pole(des)

                # Now place the poles using acker
                K = acker(sys.A, sys.B, poles)
                new = ss(sys.A - sys.B * K, sys.B, sys.C, sys.D)
                placed = pole(new)

                # Debugging code
                # diff = np.sort(poles) - np.sort(placed)
                # if not all(diff < 0.001):
                #     print("Found a problem:")
                #     print(sys)
                #     print("desired = ", poles)

                np.testing.assert_array_almost_equal(np.sort(poles),
                                                     np.sort(placed), decimal=4)
示例#4
0
 def testCtrbObsvDuality(self, matarrayin):
     A = matarrayin([[1.2, -2.3], [3.4, -4.5]])
     B = matarrayin([[5.8, 6.9], [8., 9.1]])
     Wc = ctrb(A, B)
     A = np.transpose(A)
     C = np.transpose(B)
     Wo = np.transpose(obsv(A, C))
     np.testing.assert_array_almost_equal(Wc, Wo)
示例#5
0
 def testCtrbObsvDuality(self):
     A = np.matrix("1.2 -2.3; 3.4 -4.5")
     B = np.matrix("5.8 6.9; 8. 9.1")
     Wc = ctrb(A,B);
     A = np.transpose(A)
     C = np.transpose(B)
     Wo = np.transpose(obsv(A,C));
     np.testing.assert_array_almost_equal(Wc,Wo)
示例#6
0
 def testCtrbObsvDuality(self):
     A = np.matrix("1.2 -2.3; 3.4 -4.5")
     B = np.matrix("5.8 6.9; 8. 9.1")
     Wc = ctrb(A, B)
     A = np.transpose(A)
     C = np.transpose(B)
     Wo = np.transpose(obsv(A, C))
     np.testing.assert_array_almost_equal(Wc, Wo)
    def testCtrbMIMO(self):
        A = np.array([[1., 2.], [3., 4.]])
        B = np.array([[5., 6.], [7., 8.]])
        Wctrue = np.array([[5., 6., 19., 22.], [7., 8., 43., 50.]])
        Wc = ctrb(A, B)
        np.testing.assert_array_almost_equal(Wc, Wctrue)

        # Make sure default type values are correct
        self.assertTrue(isinstance(Wc, np.ndarray))
    def testCtrbSISO(self):
        A = np.array([[1., 2.], [3., 4.]])
        B = np.array([[5.], [7.]])
        Wctrue = np.array([[5., 19.], [7., 43.]])

        Wc = ctrb(A, B)
        np.testing.assert_array_almost_equal(Wc, Wctrue)
        self.assertTrue(isinstance(Wc, np.ndarray))
        self.assertFalse(isinstance(Wc, np.matrix))
示例#9
0
    def testCtrbMIMO(self, matarrayin):
        A = matarrayin([[1., 2.], [3., 4.]])
        B = matarrayin([[5., 6.], [7., 8.]])
        Wctrue = np.array([[5., 6., 19., 22.], [7., 8., 43., 50.]])
        Wc = ctrb(A, B)
        np.testing.assert_array_almost_equal(Wc, Wctrue)

        # Make sure default type values are correct
        assert ismatarrayout(Wc)
示例#10
0
    def testCtrbSISO(self, matarrayin, matarrayout):
        A = matarrayin([[1., 2.], [3., 4.]])
        B = matarrayin([[5.], [7.]])
        Wctrue = np.array([[5., 19.], [7., 43.]])

        with check_deprecated_matrix():
            Wc = ctrb(A, B)
        assert ismatarrayout(Wc)

        np.testing.assert_array_almost_equal(Wc, Wctrue)
 def test_ctrb_siso_deprecated(self):
     A = np.array([[1., 2.], [3., 4.]])
     B = np.array([[5.], [7.]])
     
     # Check that default using np.matrix generates a warning
     # TODO: remove this check with matrix type is deprecated
     warnings.resetwarnings()
     with warnings.catch_warnings(record=True) as w:
         use_numpy_matrix(True)
         self.assertTrue(issubclass(w[-1].category, UserWarning))
         
         Wc = ctrb(A, B)
         self.assertTrue(isinstance(Wc, np.matrix))
         self.assertTrue(issubclass(w[-1].category,
                                    PendingDeprecationWarning))
         use_numpy_matrix(False)
示例#12
0
    def testConvert(self):
        """Test state space to transfer function conversion."""
        verbose = self.debug

        # 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 = ctrb(ssOriginal.A, ssOriginal.B)
                    if (np.linalg.matrix_rank(Cmat) != states):
                        if (verbose):
                            print("  skipping (not reachable)")
                        continue
                    Omat = 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 = \
                                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 = tf(num, den)

                            tforig_mag, tforig_phase, tforig_omega = \
                                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 = \
                                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 = tf(num, den)
                            tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
                                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)
def compute_gains(Q, R, W, V, dt):
    """Given LQR Q and R matrices, and Kalman W and V matrices, and sample
    time, compute optimal feedback gain and optimal filter gains."""

    data = np.empty((N, ), dtype=controller_t)

    # Loop over all speeds for which we have system dynamics
    for i in range(N):
        data['theta_R_dot'][i] = theta_R_dot[i]
        data['dt'][i] = dt
        # Convert the bike dynamics to discrete time using a zero order hold
        data['A'][i], data['B'][i], _, _, _ = cont2discrete(
            (A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt)
        data['plant_evals_d'][i] = la.eigvals(data['A'][i])
        data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt

        # Bicycle measurement matrices
        # - steer angle
        # - roll rate
        data['C_m'][i] = C_w[i, :2, :]
        # - yaw rate
        data['C_z'][i] = C_w[i, 2, :]

        A = data['A'][i]
        B = data['B'][i, :, 2].reshape((4, 1))
        C_m = data['C_m'][i]
        C_z = data['C_z'][i]

        # Controllability from steer torque
        data['ctrb_plant'][i] = ctrb(A, B)
        u, s, v = la.svd(data['ctrb_plant'][i])
        assert (np.all(s > 1e-13))

        # Solve discrete algebraic Ricatti equation associated with LQI problem
        P_c = dare(A, B, R, Q)

        # Optimal feedback gain using solution of Ricatti equation
        K_c = -la.solve(R + dot(B.T, dot(P_c, B)), dot(B.T, dot(P_c, A)))
        data['K_c'][i] = K_c
        data['A_c'][i] = A + dot(B, K_c)
        data['B_c'][i] = B
        data['controller_evals'][i] = la.eigvals(data['A_c'][i])
        data['controller_evals_c'][i] = np.log(
            data['controller_evals'][i]) / dt
        assert (np.all(abs(data['controller_evals'][i]) < 1.0))

        # Observability from steer angle and roll rate measurement
        # Note that (A, C_m * A) must be observable in the "current estimator"
        # formulation
        data['obsv_plant'][i] = obsv(A, dot(C_m, A))
        u, s, v = la.svd(data['obsv_plant'][i])
        assert (np.all(s > 1e-13))

        # Solve Riccati equation
        P_e = dare(A.T, C_m.T, V, W)
        # Compute Kalman gain
        K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V)))
        data['K_e'][i] = K_e
        data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A)
        data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e))
        data['estimator_evals'][i] = la.eigvals(data['A_e'][i])
        data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt
        # Verify that Kalman estimator eigenvalues are stable
        assert (np.all(abs(data['estimator_evals'][i]) < 1.0))

        # Closed loop state space equations
        A_cl = np.zeros((8, 8))
        A_cl[:4, :4] = A
        A_cl[:4, 4:] = dot(B, K_c)
        A_cl[4:, :4] = dot(K_e, dot(C_m, A))
        A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:]
        data['A_cl'][i] = A_cl
        data['closed_loop_evals'][i] = la.eigvals(A_cl)
        assert (np.all(abs(data['closed_loop_evals'][i]) < 1.0))

        B_cl = np.zeros((8, 1))
        B_cl[:4, 0] = B.reshape((4, ))
        B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4, ))
        data['B_cl'][i] = B_cl

        C_cl = np.hstack((C_z, np.zeros((1, 4))))
        data['C_cl'][i] = C_cl

        # Transfer functions from r to yaw rate
        num, den = ss2tf(A_cl, B_cl, C_cl, 0)
        data['w_r_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi

        # Open loop transfer function from e to yaw rate (PI loop not closed,
        # but LQR/LQG loop closed.
        inner_cl = ss(A_cl, B_cl, C_cl, 0)
        pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i] * dt]],
                      [[data['Kp_fit'][i]]])
        e_to_psi_dot = series(pi_block, inner_cl)
        num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C,
                         e_to_psi_dot.D)
        data['w_e_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi

    return data
示例#14
0
 def testCtrbSISO(self):
     A = np.matrix("1. 2.; 3. 4.")
     B = np.matrix("5.; 7.")
     Wctrue = np.matrix("5. 19.; 7. 43.")
     Wc = ctrb(A,B)
     np.testing.assert_array_almost_equal(Wc, Wctrue)
示例#15
0
 def testCtrbMIMO(self):
     A = np.matrix("1. 2.; 3. 4.")
     B = np.matrix("5. 6.; 7. 8.")
     Wctrue = np.matrix("5. 6. 19. 22.; 7. 8. 43. 50.")
     Wc = ctrb(A,B)
     np.testing.assert_array_almost_equal(Wc, Wctrue)
示例#16
0
 def testCtrbMIMO(self):
     A = np.matrix("1. 2.; 3. 4.")
     B = np.matrix("5. 6.; 7. 8.")
     Wctrue = np.matrix("5. 6. 19. 22.; 7. 8. 43. 50.")
     Wc = ctrb(A, B)
     np.testing.assert_array_almost_equal(Wc, Wctrue)
示例#17
0
 def testCtrbSISO(self):
     A = np.matrix("1. 2.; 3. 4.")
     B = np.matrix("5.; 7.")
     Wctrue = np.matrix("5. 19.; 7. 43.")
     Wc = ctrb(A, B)
     np.testing.assert_array_almost_equal(Wc, Wctrue)
示例#18
0
    def testConvert(self):
        """Test state space to transfer function conversion."""
        verbose = self.debug

        # 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 = ctrb(ssOriginal.A, ssOriginal.B)
                    if (np.linalg.matrix_rank(Cmat) != states):
                        if (verbose):
                            print("  skipping (not reachable)")
                        continue
                    Omat = 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 = \
                                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 = tf(num, den)

                            tforig_mag, tforig_phase, tforig_omega = \
                                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 = \
                                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 = tf(num, den)
                            tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
                                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)
def compute_gains(Q, R, W, V, dt):
    """Given LQR Q and R matrices, and Kalman W and V matrices, and sample
    time, compute optimal feedback gain and optimal filter gains."""

    data = np.empty((N,), dtype=controller_t)

    # Loop over all speeds for which we have system dynamics
    for i in range(N):
        data['theta_R_dot'][i] = theta_R_dot[i]
        data['dt'][i] = dt
        # Convert the bike dynamics to discrete time using a zero order hold
        data['A'][i], data['B'][i], _, _, _ = cont2discrete(
                        (A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt)
        data['plant_evals_d'][i] = la.eigvals(data['A'][i])
        data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt
        
        # Bicycle measurement matrices
        # - steer angle
        # - roll rate
        data['C_m'][i] = C_w[i, :2, :]
        # - yaw rate
        data['C_z'][i] = C_w[i, 2, :]

        A = data['A'][i]
        B = data['B'][i, :, 2].reshape((4, 1))
        C_m = data['C_m'][i]
        C_z = data['C_z'][i]

        # Controllability from steer torque
        data['ctrb_plant'][i] = ctrb(A, B)
        u, s, v = la.svd(data['ctrb_plant'][i])
        assert(np.all(s > 1e-13))

        # Solve discrete algebraic Ricatti equation associated with LQI problem
        P_c = dare(A, B, R, Q)
        
        # Optimal feedback gain using solution of Ricatti equation
        K_c = -la.solve(R + dot(B.T, dot(P_c, B)),
                                dot(B.T, dot(P_c, A)))
        data['K_c'][i] = K_c
        data['A_c'][i] = A + dot(B, K_c)
        data['B_c'][i] = B
        data['controller_evals'][i] = la.eigvals(data['A_c'][i])
        data['controller_evals_c'][i] = np.log(data['controller_evals'][i]) / dt
        assert(np.all(abs(data['controller_evals'][i]) < 1.0))

        # Observability from steer angle and roll rate measurement
        # Note that (A, C_m * A) must be observable in the "current estimator"
        # formulation
        data['obsv_plant'][i] = obsv(A, dot(C_m, A))
        u, s, v = la.svd(data['obsv_plant'][i])
        assert(np.all(s > 1e-13))

        # Solve Riccati equation
        P_e = dare(A.T, C_m.T, V, W)
        # Compute Kalman gain
        K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V)))
        data['K_e'][i] = K_e
        data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A)
        data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e))
        data['estimator_evals'][i] = la.eigvals(data['A_e'][i])
        data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt
        # Verify that Kalman estimator eigenvalues are stable
        assert(np.all(abs(data['estimator_evals'][i]) < 1.0))

        # Closed loop state space equations
        A_cl = np.zeros((8, 8))
        A_cl[:4, :4] = A
        A_cl[:4, 4:] = dot(B, K_c)
        A_cl[4:, :4] = dot(K_e, dot(C_m, A))
        A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:]
        data['A_cl'][i] = A_cl
        data['closed_loop_evals'][i] = la.eigvals(A_cl)
        assert(np.all(abs(data['closed_loop_evals'][i]) < 1.0))

        B_cl = np.zeros((8, 1))
        B_cl[:4, 0] = B.reshape((4,))
        B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4,))
        data['B_cl'][i] = B_cl

        C_cl = np.hstack((C_z, np.zeros((1, 4))))
        data['C_cl'][i] = C_cl

        # Transfer functions from r to yaw rate
        num, den = ss2tf(A_cl, B_cl, C_cl, 0)
        data['w_r_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi

        # Open loop transfer function from e to yaw rate (PI loop not closed,
        # but LQR/LQG loop closed.
        inner_cl = ss(A_cl, B_cl, C_cl, 0)
        pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i]*dt]], [[data['Kp_fit'][i]]])
        e_to_psi_dot = series(pi_block, inner_cl)
        num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C, e_to_psi_dot.D)
        data['w_e_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi




    return data
    def testConvert(self, fixedseed, states, inputs, outputs):
        """Test state space to transfer function conversion.

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

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

        tfOriginal = tf(ssOriginal)
        if (verbose):
            self.printSys(tfOriginal, 2)

        ssTransformed = ss(tfOriginal)
        if (verbose):
            self.printSys(ssTransformed, 3)

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

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

        # 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 = \
                    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 = tf(num, den)

                tforig_mag, tforig_phase, tforig_omega = \
                    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 = \
                    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, decimal=5)
                np.testing.assert_array_almost_equal(
                    ssorig_imag, ssxfrm_imag, decimal=5)

                # Make sure xform'd TF has same frequency response
                #
                num = tfTransformed.num[outputNum][inputNum]
                den = tfTransformed.den[outputNum][inputNum]
                tfxfrm = tf(num, den)
                tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
                    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, decimal=5)
                np.testing.assert_array_almost_equal(
                    ssorig_imag, tfxfrm_imag, decimal=5)