alchemyst / Skogestad-Python

Python code for "Multivariable Feedback Control"
111 stars 88 forks source link

Utils.Minimal_Realisation not working correctly #281

Closed NicVDMey closed 7 years ago

NicVDMey commented 7 years ago

This function seems to work for some inputs but not for others. The issue I have picked up specifically is that recalculating the minimal realisation can lead to a different eigenvalues of the A matrix, and therefore different poles. Test code is as follows:

import utils
import numpy

s = utils.tf([1, 0], 1)
U2Y1nd = -0.0499*(9.41*s+1)/(48.2*s**2+23.1*s+1)
U1Y2nd = 0.203/(10*s+1)
U2Y2nd = 0.0463*(27.5*s+1)/(27.3*s**2+10.4*s+1)
U3Y2nd = 0.121/(7.71*s+1)
U4Y2nd = 0.156/(2.25*s+1)
U3Y3nd = -1/(0.04*s**2+0.4*s+1)
U1Y4nd = 0.549*(-4.65*s+1)/(1.06*s**2+2.05*s+1)
U2Y4nd = -0.296*(18*s+1)/(0.58*s**2+2.76*s+1)
U4Y4nd = -1.24*(-16.1*s+1)/(21.8*s**2+9.33*s+1)

Gnd = utils.mimotf([[0, U2Y1nd, 0, 0],[U1Y2nd, U2Y2nd, U3Y2nd, U4Y2nd], [0, 0, U3Y3nd, 0], [U1Y4nd, U2Y4nd, 0, U4Y4nd]])
A, B, C, _ = utils.tf2ss(Gnd)

Amin1, _, _ = utils.minimal_realisation(A, B, C)
Amin2, _, _ = utils.minimal_realisation(A, B, C)

Eigs1, _ = numpy.linalg.eig(Amin1)
Eigs2, _ = numpy.linalg.eig(Amin2)

assert numpy.array_equal(sorted(Eigs1, key=lambda x: x.real), sorted(Eigs2, key=lambda x: x.real)), "Eigenvalues should be equal" 

I believe the problem is in the remove_uncontrollable_or_unobservable_states function, specifically in the following lines:

    elif uncontrollable is False and unobservable is True:
        P[0:rank, :] = con_or_obs_matrix[0:rank, :]

        # matrix will replace the dependent columns in P to make P invertible
        replace_matrix = numpy.matrix(numpy.random.random((m, n_states)))

        # make P invertible
        P[rank:n_states, :] = replace_matrix

        P_inv = numpy.linalg.inv(P)

    A_new = P*a*P_inv
    A_new = numpy.delete(A_new, numpy.s_[rank:n_states], 1)
    A_new = numpy.delete(A_new, numpy.s_[rank:n_states], 0)

    B_new = P*b
    B_new = numpy.delete(B_new, numpy.s_[rank:n_states], 0)

    C_new = c*P_inv
    C_new = numpy.delete(C_new, numpy.s_[rank:n_states], 1)

given that we entered the elif part of the if statement. I suspect that the issue is in the deletes, are these supposed to be different depending on which path we went through in the if statement? Unfortunately I don't know what the algorithm should look like.