PEAK_MIMO.py

from __future__ import print_function
import numpy as np
import scipy.linalg as sc_lin
import matplotlib.pyplot as plt

from utils import poles, zeros
from utilsplot import plot_freq_subplot, ref_perfect_const_plot


# TODO redefine this function with utils and utilsplot functions
def PEAK_MIMO(w_start, w_end, error_poles_direction, wr, deadtime_if=0):
    """
    This function is for multivariable system analysis of controllability.
    gives:
    minimum peak values on S and T with or without deadtime
    R is the expected worst case reference change, with condition that
    ||R||2<= 2 wr is the frequency up to where reference tracking is required
    enter value of 1 in deadtime_if if system has dead time

    Parameters
    ----------
    var : type
        Description (optional).

    Returns
    -------
    var : type
        Description.
    """

    # TODO use mimotf functions
    Zeros_G = zeros(G)
    Poles_G = poles(G)
    print('Poles: ', Zeros_G)
    print('Zeros: ', Poles_G)

    # Just to save unnecessary calculations that is not needed
    # Sensitivity peak of closed loop. eq 6-8 pg 224 skogestad

    if np.sum(Zeros_G) != 0:
        if np.sum(Poles_G) != 0:

            # Two matrices to save all the RHP zeros and poles directions
            yz_direction = np.matrix(np.zeros([G(0.001).shape[0],
                                               len(Zeros_G)]))
            yp_direction = np.matrix(np.zeros([G(0.001).shape[0],
                                               len(Poles_G)]))

            for i in range(len(Zeros_G)):

                [U, S, V] = np.linalg.svd(G(Zeros_G[i]+error_poles_direction))
                yz_direction[:, i] = U[:, -1]

            for i in range(len(Poles_G)):
                # Error_poles_direction is to to prevent the numerical
                # method from breaking
                [U, S, V] = np.linalg.svd(G(Poles_G[i]+error_poles_direction))
                yp_direction[:, i] = U[:, 0]

            yz_mat1 = (np.matrix(np.diag(Zeros_G)) *
                       np.matrix(np.ones([len(Zeros_G), len(Zeros_G)])))
            yz_mat2 = yz_mat1.T

            Qz = (yz_direction.H*yz_direction)/(yz_mat1+yz_mat2)

            yp_mat1 = np.matrix(np.diag(Poles_G)) * \
                      np.matrix(np.ones([len(Poles_G), len(Poles_G)]))
            yp_mat2 = yp_mat1.T

            Qp = (yp_direction.H*yp_direction)/(yp_mat1+yp_mat2)

            yzp_mat1 = np.matrix(np.diag(Zeros_G)) * \
                       np.matrix(np.ones([len(Zeros_G), len(Poles_G)]))
            yzp_mat2 = np.matrix(np.ones([len(Zeros_G), len(Poles_G)])) * \
                       np.matrix(np.diag(Poles_G))

            Qzp = yz_direction.H*yp_direction/(yzp_mat1-yzp_mat2)

            if deadtime_if == 0:
                # This matrix is the matrix from which the SVD is going to
                # be done to determine the final minimum peak
                pre_mat = (sc_lin.sqrtm((np.linalg.inv(Qz))) * 
                           Qzp*(sc_lin.sqrtm(np.linalg.inv(Qp))))

                # Final calculation for the peak value
                Ms_min = np.sqrt(1+(np.max(np.linalg.svd(pre_mat)[1]))**2)
                print('')
                print('Minimum peak values on T and S without deadtime')
                print('Ms_min = Mt_min = ', Ms_min)
                print('')

            # Skogestad eq 6-16 pg 226 using maximum deadtime per output
            # channel to give tightest lowest bounds
            if deadtime_if == 1:
                # Create vector to be used for the diagonal deadtime matrix
                # containing each outputs' maximum dead time
                # this would ensure tighter bounds on T and S
                # the minimum function is used because all stable systems
                # have dead time with a negative sign

                dead_time_vec_max_row = np.zeros(deadtime()[0].shape[0])

                for i in range(deadtime()[0].shape[0]):
                    dead_time_vec_max_row[i] = np.max(deadtime()[0][i, :])

                def Dead_time_matrix(s, dead_time_vec_max_row):

                    dead_time_matrix = np.diag(
                        np.exp(np.multiply(dead_time_vec_max_row, s)))
                    return dead_time_matrix

                Q_dead = np.zeros([G(0.0001).shape[0], G(0.0001).shape[0]])

                for i in range(len(Poles_G)):
                    for j in range(len(Poles_G)):
                        denominator_mat = ((np.conjugate(yp_direction[:, i]))
                           *Dead_time_matrix(Poles_G[i], dead_time_vec_max_row)
                           *Dead_time_matrix(Poles_G[j], dead_time_vec_max_row)
                           *yp_direction[:, j]).T

                        numerator_mat = Poles_G[i]+Poles_G[i]

                        Q_dead[i, j] = denominator_mat/numerator_mat

                # Calculating the Mt_min with dead time
                lambda_mat = (sc_lin.sqrtm(np.linalg.pinv(Q_dead))*
                             (Qp+Qzp*np.linalg.pinv(Qz)*
                             (np.transpose(np.conjugate(Qzp))))*
                             sc_lin.sqrtm(np.linalg.pinv(Q_dead)))

                Ms_min = np.real(np.max(np.linalg.eig(lambda_mat)[0]))
                print('')
                print('Minimum peak values on T and S without dead time')
                print('Dead time per output channel is for the worst case '
                      'dead time in that channel')
                print('Ms_min = Mt_min = ', Ms_min)
                print('')

        else:
            print('')
            print('Minimum peak values on T and S')
            print('No limits on minimum peak values')
            print('')

    # Eq 6-48 pg 239 for plant with RHP zeros
    # Checking alignment of disturbances and RHP zeros
    RHP_alignment = [
        np.abs(np.linalg.svd(G(RHP_Z+error_poles_direction))[0][:, 0].H*
        np.linalg.svd(Gd(RHP_Z+error_poles_direction))[1][0]*
        np.linalg.svd(Gd(RHP_Z+error_poles_direction))[0][:, 0])
        for RHP_Z in Zeros_G
        ]

    print('Checking alignment of process output zeros to disturbances')
    print('These values should be less than 1')
    print(RHP_alignment)
    print('')

    # Checking peak values of KS eq 6-24 pg 229 np.linalg.svd(A)[2][:, 0]
    # Done with less tight lower bounds
    KS_PEAK = [np.linalg.norm(
        np.linalg.svd(G(RHP_p+error_poles_direction))[2][:, 0].H*
        np.linalg.pinv(G(RHP_p+error_poles_direction)), 2)
        for RHP_p in Poles_G
        ]
    KS_max = np.max(KS_PEAK)

    print('Lower bound on K')
    print('KS needs to larger than ', KS_max)
    print('')

    # Eq 6-50 pg 240 from Skogestad
    # Eg 6-50 pg 240 from Skogestad for simultanious disturbance matrix
    # Checking input saturation for perfect control for disturbance rejection
    # Checking for maximum disturbance just at steady state

    [U_gd, S_gd, V_gd] = np.linalg.svd(Gd(0.000001))
    y_gd_max = np.max(S_gd)*U_gd[:, 0]
    mod_G_gd_ss = np.max(np.linalg.inv(G(0.000001))*y_gd_max)

    print('Perfect control input saturation from disturbances')
    print('Needs to be less than 1 ')
    print('Max Norm method')
    print('Checking input saturation at steady state')
    print('This is done by the worse output direction of Gd')
    print(mod_G_gd_ss)
    print('')

    print('Figure 1 is for perfect control for simultaneous disturbances')
    print('All values on each of the graphs should be smaller than 1')
    print('')

    print('Figure 2 is the plot of G**1 gd')
    print('The values of this plot needs to be smaller or equal to 1')
    print('')

    w = np.logspace(w_start, w_end, 100)

    mod_G_gd = np.zeros(len(w))
    mod_G_Gd = np.zeros([np.shape(G(0.0001))[0], len(w)])

    for i in range(len(w)):
        [U_gd, S_gd, V_gd] = np.linalg.svd(Gd(1j*w[i]))
        gd_m = np.max(S_gd)*U_gd[:, 0]
        mod_G_gd[i] = np.max(np.linalg.pinv(G(1j*w[i]))*gd_m)

        mat_G_Gd = np.linalg.pinv(G(w[i]))*Gd(w[i])
        for j in range(np.shape(mat_G_Gd)[0]):
            mod_G_Gd[j, i] = np.max(mat_G_Gd[j, :])

    # Def for subplotting all the possible variations of mod_G_Gd


    plot_freq_subplot(plt, w, np.ones([2, len(w)]),
                      'Perfect control Gd', 'r', 1)
    plot_freq_subplot(plt, w, mod_G_Gd, 'Perfect control Gd', 'b', 1)

    plt.figure(2)
    plt.title('Input Saturation for perfect control |inv(G)*gd|<= 1')
    plt.xlabel('w')
    plt.ylabel('|inv(G)* gd|')
    plt.semilogx(w, mod_G_gd)
    plt.semilogx([w[0], w[-1]], [1, 1])
    plt.semilogx(w[0], 1.1)

    print('Figure 3 is disturbance condition number')
    print('A large number indicates that the disturbance'
          'is in a bad direction')
    print('')
    
    # Eq 6-43 pg 238 disturbance condition number
    # this in done over a frequency range to see if there are possible
    # problems at higher frequencies finding yd
    dist_condition_num = [
        np.linalg.svd(G(w_i))[1][0]*
        np.linalg.svd(np.linalg.pinv(G(w_i))[1][0]*
        np.linalg.svd(Gd(w_i))[1][0]*
        np.linalg.svd(Gd(w_i))[0][:, 0])[1][0] for w_i in w
        ]

    plt.figure(3)
    plt.title('yd Condition number')
    plt.ylabel('condition number')
    plt.xlabel('w')
    plt.loglog(w, dist_condition_num)

    print('Figure 4 is the singular value of an specific output with input '
          'and disturbance direction vector')
    print('The solid blue line needs to be large than the red line')
    print('This only needs to be checked up to frequencies where |u**H gd| >1')
    print('')

    # Checking input saturation for acceptable control  disturbance rejection
    # Equation 6-55 pg 241 in Skogestad
    # Checking each singular values and the associated input vector with
    # output direction vector of Gd just for square systems for now

    # Revised method including all the possibilities of outputs i
    store_rhs_eq = np.zeros([np.shape(G(0.0001))[0], len(w)])
    store_lhs_eq = np.zeros([np.shape(G(0.0001))[0], len(w)])

    for i in range(len(w)):
        for j in range(np.shape(G(0.0001))[0]):
            store_rhs_eq[j, i] = (np.abs(np.linalg.svd(G(w[i]))[2][:, j].H*
                                  np.max(np.linalg.svd(Gd(w[i]))[1])*
                                  np.linalg.svd(Gd(w[i]))[0][:, 0])-1)
            store_lhs_eq[j, i] = sc_lin.svd(G(w[i]))[1][j]

    plot_freq_subplot(plt, w, store_rhs_eq,
                      'Acceptable control eq6-55', 'r', 4)
    plot_freq_subplot(plt, w, store_lhs_eq,
                      'Acceptable control eq6-55', 'b', 4)

    print('Figure 5 is to check input saturation for reference changes')
    print('Red line in both graphs needs to be larger than the blue '
          'line for values w < wr')
    print('Shows the wr up to where control is needed')
    print('')

    # Checking input saturation for perfect control with reference change
    # Eq 6-52 pg 241

    # Checking input saturation for perfect control with reference change
    # Another equation for checking input saturation with reference change
    # Eq 6-53 pg 241

    plt.figure(5)
    ref_perfect_const_plot(G, reference_change(), 0.01, w_start, w_end)

    print('Figure 6 is the maximum and minimum singular values of G over '
          'a frequency range')
    print('Figure 6 is also the maximum and minimum singular values of Gd '
          'over a frequency range')
    print('Blue is the minimum values and Red is the maximum singular values')
    print('Plot of Gd should be smaller than 1 else control is needed at '
          'frequencies where Gd is bigger than 1')
    print('')

    # Checking input saturation for acceptable control with reference change
    # Added check for controllability is the minimum and maximum singular
    # values of system transfer function matrix
    # as a function of frequency condition number added to check for how
    # prone the system would be to uncertainty

    singular_min_G = [np.min(np.linalg.svd(G(1j*w_i))[1]) for w_i in w]
    singular_max_G = [np.max(np.linalg.svd(G(1j*w_i))[1]) for w_i in w]
    singular_min_Gd = [np.min(np.linalg.svd(Gd(1j*w_i))[1]) for w_i in w]
    singular_max_Gd = [np.max(np.linalg.svd(Gd(1j*w_i))[1]) for w_i in w]
    condition_num_G = [np.max(np.linalg.svd(G(1j*w_i))[1])/
                       np.min(np.linalg.svd(G(1j*w_i))[1]) for w_i in w]

    plt.figure(6)
    plt.subplot(311)
    plt.title('min_S(G(jw)) and max_S(G(jw))')
    plt.loglog(w, singular_min_G, 'b')
    plt.loglog(w, singular_max_G, 'r')

    plt.subplot(312)
    plt.title('Condition number of G')
    plt.loglog(w, condition_num_G)

    plt.subplot(313)
    plt.title('min_S(Gd(jw)) and max_S(Gd(jw))')
    plt.loglog(w, singular_min_Gd, 'b')
    plt.loglog(w, singular_max_Gd, 'r')
    plt.loglog([w[0], w[-1]], [1, 1])

    plt.show()

    return Ms_min

# only executed when called directly, not executed when imported
if __name__ == '__main__':

    def G(s):
        # give the transfer matrix of the system
        return np.matrix([[1 / (s + 1), 1],
                          [1 / (s + 2) ** 2, (s - 1) / (s + 2)]])

    def Gd(s):
        return np.matrix([[1 / (s + 1)],
                          [1 / (s + 20)]])

    def reference_change():
        # Reference change matrix/vector for use in eq
        # 6-52 pg 242 to check input saturation
        R = np.matrix([[1, 0], [0, 1]])
        return R/np.linalg.norm(R, 2)

    def deadtime():
        # vector of the deadtime of the system
        # individual time delays
        dead_G = np.matrix([[0, -2], [-1, -4]])
        dead_Gd = np.matrix([])
        return dead_G, dead_Gd

    PEAK_MIMO(-4, 5, 0.00001, 0.1, 1)