## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal

### Package Content

Name | Description
--- | ---
assignPolesMI_rob_old |
wrapper_modifyX_old | Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm
modifyX_old | Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm
assignPolesMI_rob | Modified KNV algorithm. Works like MATLAB's place.m
assignPolesMI_rob2 | Modified KNV algorithm. Works like MATLAB's place.m
assignPolesSI_rq | RQ implementation of a recursiv single-input pole assignment problem
xBase | Compute the eigenvector bases according to Kautsky algorithm
calcK | Computes the feedback matrix from the assigned eigenvalues, closed loop eigenvectors and the B matrix factorization
modifyX | Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm
wrapper_calcK | Computes the feedback matrix from the assigned eigenvalues, closed loop eigenvectors and the B matrix factorization
wrapper_modifyX | Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm
wrapper_xBase | Compute the eigenvector bases according to Kautsky algorithm

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.assignPolesMI_rob_old

### Information

Extends from Modelica.Icons.Function (Icon for a function).

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | A[:, size(A, 1)] |  | system matrix
Real | B[size(A, 1), :] |  | control input matrix
Complex | gamma[size(A, 1)] |  |
Boolean | IniX | false | Initial values of X are provided
Complex | Xini[size(A, 1), size(A, 1)] | fill(Complex(0), size(A, 1),... | Initial values of the eigenvectors X

### Outputs

Type | Name | Description
--- | --- | ---
Real | K[size(B, 2), size(A, 1)] | feedback matrix
Complex | evX[:, :] | eigen vectors of the closed loop system

### Modelica definition

    function assignPolesMI_rob_old
      extends Modelica.Icons.Function;

      import Modelica;
      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.StateSpace;
      import Modelica_LinearSystems2.Math.Matrices;
      import Modelica_LinearSystems2.Math.Complex;
      import matMul = Modelica_LinearSystems2.Math.Complex.Matrices.matMatMul;
      import Modelica_LinearSystems2.Math.Complex.Matrices.matVecMul;
      import Modelica_LinearSystems2.Math.Complex.Internal.C_transpose;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;
      import Modelica.Utilities.Streams.print;

      input Real A[:,size(A, 1)] "system matrix";
      input Real B[size(A, 1),:] "control input matrix";
      input Complex gamma[size(A, 1)];
      input Boolean IniX=false "Initial values of X are provided";
      input Complex Xini[size(A, 1),size(A, 1)]=fill(Complex(0),size(A, 1),size(A, 1))
        "Initial values of the eigenvectors X";

      output Real K[size(B, 2),size(A, 1)] "feedback matrix";
      output Complex evX[:,:] "eigen vectors of the closed loop system";
    protected
      Complex AC[size(A, 1),size(A, 2)]=Complex(1)*A;
      Complex Lambda[size(A, 1),size(A, 1)];
      Real Ur[:,:];
      Real Vr[:,:];
      Complex U0[:,:];
      Complex U1[:,:];
      Complex U1T[:,:];
      Complex X[size(A, 1),size(A, 2)]=Xini;
      Complex Xj[size(A, 1),size(A, 1)-1];
      Complex XC[size(A, 1),size(A, 2)];
      Complex XC2[size(A, 1),size(A, 2)];
      Complex Z[:,:];
      Complex M[size(A, 1),size(A, 1)];
      Complex KC[:,:];
      Complex QX[:,:];
      Complex C[:,:];
      Complex Cc[:,:];
      Complex Sr[:,:];
      Complex ST[:,:];

      Complex gammaSorted[size(gamma, 1)];
      Complex gammaSorted2[size(gamma, 1)];
      Real sigmaB[:];
      Complex ev[size(A, 1)];

      Complex y[:];

      Real condX2;
      Real norm_y;
      Integer rankB;
      Integer nx=size(A, 1);
      Integer numberOfRealEigenvalues;
      Integer numberOfComplexPairs;
      Integer i;
      Integer l1;
      Integer l2;
      Integer k;
      Integer idx;
      Real eps=Modelica.Constants.eps;
      Integer maxSteps=3;

      Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.assignPolesMI_rob_old.subSpace
        subS[                                                                         size(gamma,1)];

      Complex a;
      Complex MM[:,:];
      Integer cnt;
      StateSpace ss=StateSpace(A=A, B=B, C=zeros(1,nx), D=zeros(1,size(B,2)));

    algorithm
      //check controllability
      assert(StateSpace.Analysis.isControllable(ss),"Poles cannot be placed since system is not controllable");

      // sort eigenvalues to [real ev, complex ev(im>0), conj(complex ev(im>0))]
      (gammaSorted2,numberOfRealEigenvalues) :=
        Modelica_LinearSystems2.Internal.reorderZeros(gamma);
      gammaSorted := gammaSorted2;
      numberOfComplexPairs := integer((nx - numberOfRealEigenvalues)/2);
      for i in numberOfRealEigenvalues + 1:numberOfRealEigenvalues + numberOfComplexPairs loop
        gammaSorted[i] := if Im(gammaSorted2[2*i-numberOfRealEigenvalues-1]) > 0 then gammaSorted2[2*i-numberOfRealEigenvalues-1] else Complex.conj(gammaSorted2[2*i-numberOfRealEigenvalues-1]);
        gammaSorted[i + numberOfComplexPairs] := Complex.conj(gammaSorted[i]);
      end for;

      for i in 1:nx loop
        Lambda[i, i] := gammaSorted[i];
      end for;

    //   (sigmaB,Q,R) := Matrices.C_singularValues(BC);
    //   rankB := 0;
    //   i := size(sigmaB, 1);
    //   while i > 0 loop
    //     if sigmaB[i] > 1e-10 then
    //       rankB := i;
    //       i := 0;
    //     end if;
    //     i := i - 1;
    //   end while;

      (sigmaB,Ur,Vr) := Modelica.Math.Matrices.singularValues(B);
      rankB := 0;
      i := size(sigmaB, 1);
      while i > 0 loop
        if sigmaB[i] > 1e-10 then
          rankB := i;
          i := 0;
        end if;
        i := i - 1;
      end while;

    //    R := C_transpose(R);
    //    Z := R[1:rankB, 1:size(B, 2)];
    //   for l1 in 1:rankB loop
    //     for l2 in 1:size(B, 2) loop
    //       Z[l1, l2] := Z[l1, l2]/sigmaB[l2];
    //     end for;
    //   end for;

       Z := fill(Complex(0), size(B, 2), rankB);
       for l1 in 1:rankB loop
         for l2 in 1:size(B, 2) loop
    //       Z[l1, l2] := Complex.conj(R[l2, l1])/sigmaB[l2];
           Z[l1, l2] := Complex((Vr[l2, l1])/sigmaB[l2]);
         end for;
       end for;

    //   U0 := Q[:, 1:rankB];
    //   U1 := Q[:, rankB + 1:nx];
    //   U1T := C_transpose(U1);

      U0 := Complex(1)*Ur[:, 1:rankB];
      U1 := Complex(1)*Ur[:, rankB + 1:nx];
      U1T := Complex(1)*transpose(Ur[:, rankB + 1:nx]);

      condX2 := eps + 1;

      if numberOfComplexPairs > 0 then
    //      (sigmaM,Qu,Ru) := Matrices.C_singularValues([U1T;  matMul(U1T,Complex(1)*A)]);
    //       rank_ := 0;
    //          i := size(sigmaM,1);
    //        while i > 0 loop
    //          if sigmaM[i] > 1e-10 then
    //            rank_ := i;
    //            i := 0;
    //          end if;
    //          i := i - 1;
    //        end while;
    //      Ru := C_transpose(Ru);
    //      Sr := Ru[:,rank_+1:nx];

         Sr :=  Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_nullspace(
                                     [U1T;  matMul(U1T,Complex(1)*A)]);
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Re(Sr),6,"ReSr");
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Im(Sr),6,"ImSr");
      else
        Sr := fill(Complex(0),nx,0);
      end if;

      for l1 in 1:nx - numberOfComplexPairs loop

        AC := Complex(-1)*A;

        for l2 in 1:nx loop
          AC[l2, l2] := AC[l2, l2] + gammaSorted[l1];
        end for;

        C := matMul(U1T, AC);

        Cc := Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_nullspace(
                                   [C; C_transpose(Sr)]);

    //     if numberOfComplexPairs > 0 and l1 > numberOfRealEigenvalues then
    // //       (sigmaC,QC,RC) := Matrices.C_singularValues([C; C_transpose(Sr)]);
    // //       rankC := 0;
    // //       i := size(sigmaC, 1);
    // //       while i > 0 loop
    // //         if sigmaC[i] > 1e-10 then
    // //           rankC := i;
    // //           i := 0;
    // //         end if;
    // //         i := i - 1;
    // //       end while;
    // //       RC := C_transpose(RC);
    // //       subS[l1].S := [RC[:, rankC + 1:nx],Sr];
    //
    //       RC := Matrices.C_nullspace([C; C_transpose(Sr)]);
    //       subS[l1].S := [RC,Sr];
    //     else
    // //       (sigmaC,QC,RC) := Modelica_LinearSystems2.Math.Matrices.C_singularValues(C);
    // //       rankC := 0;
    // //       i := size(sigmaC, 1);
    // //       while i > 0 loop
    // //         if sigmaC[i] > 1e-10 then
    // //           rankC := i;
    // //           i := 0;
    // //         end if;
    // //         i := i - 1;
    // //       end while;
    // //       RC := C_transpose(RC);
    // //       subS[l1].S := RC[:, rankC + 1:nx];
    //
    //       subS[l1].S := Matrices.C_nullspace(C);
    //     end if;

        subS[l1].S := if l1 > numberOfRealEigenvalues then [Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_nullspace(
                                                                                 [C; C_transpose(Sr)]),Sr] else Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_nullspace(
                                                                                                        C);

    //    subS[l1].S := Matrices.C_nullspace(C);
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Re(subS[l1].S),6,"ResubS[l1].S");
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Im(subS[l1].S),6,"ImsubS[l1].S");

    // MM := matMul(C,Complex.conj(Matrices.C_nullspace(Cc)));
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Re(MM),6,"ReMM");
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Im(MM),6,"ImMM");

    // MM := matMul(C,Sr);
    // //MM := matMul(C_transpose(Sr),Cc);
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Re(MM),6,"ReMM");
    // Modelica_LinearSystems2.Math.Matrices.printMatrix(Im(MM),6,"ImMM");
    end for;

    if not IniX then
    // initialization of X according to Byers
      for l1 in 1:nx - numberOfComplexPairs loop
        y := fill(Complex(0), nx);
        for l2 in 1:size(subS[l1].S, 2) loop
          y := X[:, l1] + X[:, l1] + subS[l1].S[:, l2];
        end for;
        y := y/Complex.Vectors.norm(y);
        for l2 in 1:nx loop
          X[l2, l1] := y[l2];
        end for;
      end for;
      for l1 in 1:numberOfComplexPairs loop
        for l2 in 1:nx loop
          X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
            Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
        end for;
      end for;

     // initialization of X according to place.m
       for l1 in 1:nx - numberOfComplexPairs loop
         for l2 in 1:nx loop
           X[l2, l1] := subS[l1].S[l2, 1];
         end for;
       end for;
       for l1 in 1:numberOfComplexPairs loop
         for l2 in 1:nx loop
           X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
             Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
         end for;
       end for;
    end if;
    // eigenvector modification
      k := 0;
      while (k < maxSteps) loop
        k := k + 1;

        for l1 in 1:nx - numberOfComplexPairs loop
          if l1 == 1 then
            for l2 in 1:nx loop
              for l3 in 1:nx - 1 loop
                Xj[l2, l3] := X[l2, l3 + 1];
              end for;
            end for;
          else
            for l2 in 1:nx loop
              for l3 in 1:l1 - 1 loop
                Xj[l2, l3] := X[l2, l3];
              end for;
              for l3 in l1:nx - 1 loop
                Xj[l2, l3] := X[l2, l3 + 1];
              end for;
            end for;
          end if;

          QX := Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_QR2(
                                                            Xj);
          ST := C_transpose(subS[l1].S);
          y := matVecMul(ST, QX[:, nx]);
          norm_y := Complex.Vectors.norm(y);
          y := matVecMul(subS[l1].S, y)/norm_y;

    //        if l1 > numberOfRealEigenvalues and Complex.'abs'(Complex.Vectors.multiply(y,Complex.conj(y)))>0.9 then
    //          idx := 1 + rem(k, size(subS[l1].S, 2) - size(Sr, 2));
    //          print(" k = "+String(k)+", l1 = "+String(l1)+", idx = " + String(idx));
    //          y := (y + subS[l1].S[:, idx])/sqrt(2);
    //        end if;

          for l2 in 1:nx loop
            X[l2, l1] := y[l2];
          end for;

          if l1 > numberOfRealEigenvalues then
            for l2 in 1:nx loop
              X[l2, l1 + numberOfComplexPairs] := Complex.conj(y[l2]);
            end for;
          end if;

            end for;
    condX2 := Modelica_LinearSystems2.WorkInProgress.Math.Complex.Matrices.conditionNumber(
                                               X);

    //print("\ncondX2 = "+String(condX2));
      end while;

      XC := C_transpose(X);
      XC2 := C_transpose(matMul(X, Lambda));
      M := Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_solve2(
                                                          XC, XC2);
      M := C_transpose(M);

      for l2 in 1:nx loop
        for l3 in 1:nx loop
          M[l2, l3] := M[l2, l3] - A[l2, l3];
        end for;
      end for;

      KC := matMul(Z, matMul(C_transpose(U0), M));
      K := -Re(KC);
      evX := X;

      ev := Complex.eigenValues(A - B*K);
    //    Complex.Vectors.print("gammaSorted", gammaSorted);
    //    Complex.Vectors.print("ev", ev);

    public
      encapsulated record subSpace
        import Modelica;
        import Modelica_LinearSystems2;
        extends Modelica.Icons.Record;
        Modelica_LinearSystems2.Math.Complex S[:,:];
      end subSpace;

    end assignPolesMI_rob_old;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_modifyX_old

Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | X_real[:, size(X_real, 1)] |  | Eigenvector matrix, real part
Real | X_imag[size(X_real, 1), size(X_real, 2)] |  | Eigenvector matrix, imaginary part
Integer | n |  | Order of X
Real | S_real[size(X_real, 1), :] |  | Eigenvector bases, real part
Real | S_imag[size(S_real, 1), size(S_real, 2)] |  | Eigenvector bases, imaginary part
Integer | m |  | Rank of the system input matrix B; S_real and S_imag must have n*m columns
Integer | ncp |  | number of complex pairs
Integer | steps |  | Number of iterations

### Outputs

Type | Name | Description
--- | --- | ---
Real | Xm_real[size(X_real, 1), size(X_real, 2)] |
Real | Xm_imag[size(X_imag, 1), size(X_imag, 2)] |

### Modelica definition

    function wrapper_modifyX_old
      "Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm"
      input Real X_real[:,size(X_real, 1)] "Eigenvector matrix, real part";
      input Real X_imag[size(X_real, 1),size(X_real, 2)]
        "Eigenvector matrix, imaginary part";
      input Integer n "Order of X";
      input Real S_real[size(X_real, 1),:] "Eigenvector bases, real part";
      input Real S_imag[size(S_real, 1),size(S_real, 2)]
        "Eigenvector bases, imaginary part";
      input Integer m
        "Rank of the system input matrix B; S_real and S_imag must have n*m columns";
      input Integer ncp "number of complex pairs";
      input Integer steps "Number of iterations";

      output Real Xm_real[size(X_real, 1),size(X_real, 2)]=X_real;
      output Real Xm_imag[size(X_imag, 1),size(X_imag, 2)]=X_imag;

    external"FORTRAN 77" c_inter_modifyX(
        Xm_real,
        Xm_imag,
        n,
        S_real,
        S_imag,
        m,
        ncp,
        steps);

    end wrapper_modifyX_old;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.modifyX_old

Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Complex | X[:, size(X, 1)] |  | Complex eigenvector matrix
Complex | S[size(X, 1), :] |  | Complex eigenvector matrix
Integer | m |  | Rank of the system input matrix B; S_real and S_imag must have n*m columns
Integer | ncp |  | number of complex pairs
Integer | steps |  | Number of iterations

### Outputs

Type | Name | Description
--- | --- | ---
Complex | Xm[size(X, 1), size(X, 2)] |

### Modelica definition

    function modifyX_old
      "Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm"

      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.Math.Complex;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;

      input Complex X[:,size(X,1)] "Complex eigenvector matrix";
      input Complex S[size(X,1),:] "Complex eigenvector matrix";
      input Integer m
        "Rank of the system input matrix B; S_real and S_imag must have n*m columns";
      input Integer ncp "number of complex pairs";
      input Integer steps "Number of iterations";

      output Complex Xm[size(X, 1),size(X, 2)];

    protected
       Complex j=Modelica_LinearSystems2.Math.Complex.j();
       Integer n=size(X,1);
       Real X_real[n,n]=Re(X) "Eigenvector matrix, real part";
       Real X_imag[n,n]=Im(X) "Eigenvector matrix, imaginary part";
       Real S_real[n,m*(n-ncp)]=Re(S) "Eigenvector bases, real part";
       Real S_imag[n,m*(n-ncp)]=Im(S) "Eigenvector bases, imaginary part";

      Real Xm_real[n,n];
      Real Xm_imag[n,n];

      Integer i;
      Integer ii;

    algorithm
      (Xm_real, Xm_imag) :=Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_modifyX_old(
                                                                                       X_real, X_imag, n, S_real, S_imag, m, ncp, steps);
      for i in 1:n loop
        for ii in 1:n loop
          Xm[i,ii] := Complex(Xm_real[i,ii],Xm_imag[i,ii]);
        end for;
      end for;
    end modifyX_old;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.assignPolesMI_rob

Modified KNV algorithm. Works like MATLAB's place.m

### Information

Extends from Modelica.Icons.Function (Icon for a function).

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | A[:, size(A, 1)] |  | system matrix
Real | B[size(A, 1), :] |  | control input matrix
Complex | gamma[size(A, 1)] |  |
Boolean | IniX | false | Initial values of X are provided
Complex | Xini[size(A, 1), size(A, 1)] | fill(Complex(0), size(A, 1),... | Initial values of the eigenvectors X

### Outputs

Type | Name | Description
--- | --- | ---
Real | K[size(B, 2), size(A, 1)] | feedback matrix
Complex | X[size(A, 1), size(A, 2)] | eigen vectors of the closed loop system

### Modelica definition

    function assignPolesMI_rob
      "Modified KNV algorithm. Works like MATLAB's place.m"
      extends Modelica.Icons.Function;

      import Modelica;
      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.StateSpace;
      import Modelica_LinearSystems2.Math.Matrices;
      import Modelica_LinearSystems2.Math.Complex;
      import matMul = Modelica_LinearSystems2.Math.Complex.Matrices.matMatMul;
      import Modelica_LinearSystems2.Math.Complex.Matrices.matVecMul;
      import Modelica_LinearSystems2.Math.Complex.Internal.C_transpose;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;
      import Modelica.Utilities.Streams.print;

      input Real A[:,size(A, 1)] "system matrix";
      input Real B[size(A, 1),:] "control input matrix";
      input Complex gamma[size(A, 1)];
      input Boolean IniX=false "Initial values of X are provided";
      input Complex Xini[size(A, 1),size(A, 1)]=fill(Complex(0), size(A, 1), size(A, 1))
        "Initial values of the eigenvectors X";

      output Real K[size(B, 2),size(A, 1)] "feedback matrix";
      output Complex X[size(A, 1),size(A, 2)]=Xini
        "eigen vectors of the closed loop system";

    protected
      Real U0[:,:];
      Real Z[:,:];
      Complex S[:,:];
      Integer maxSteps=5;
      Integer numberOfComplexPairs;
      Integer rankB;

    //   Complex AC[size(A, 1),size(A, 2)]=Complex(1)*A;
    //   Complex Lambda[size(A, 1),size(A, 1)];
    //   Real Ur[:,:];
    //   Real Vr[:,:];
    //   Complex U1T[:,:];
    //   Complex Xjj[size(A, 1),size(A, 1)];
    //    Complex XC[size(A, 1),size(A, 2)];
    //    Complex XC2[size(A, 1),size(A, 2)];
    //    Complex MM[size(A, 1),size(A, 1)];
    //    Real M[size(A, 1),size(A, 1)];
    //   Complex KC[:,:];
    //   Complex QX[:,:];
    //   Complex C[:,:];
    //   Complex Cc[:,:];
    //   Complex Sr[:,:];
    //   Real Srr[:,:];
    //   Real U1Tr[:,:];
    //   Complex ST[:,:];

    //  Complex S2[size(A, 1),size(B, 2)];
    //  Real sigmaB[:];

    //  Complex y[:];
    //  Modelica_LinearSystems2.StateSpace.Internal.assignPolesMI_rob.subSpace subS[size(gamma,1)];

    //  Complex a;
    //  Integer cnt;

      Complex gammaSorted[size(gamma, 1)];
      Complex gammaSorted2[size(gamma, 1)];

      Real condX2;
      Real norm_y;

      Integer nx=size(A, 1);
      Integer numberOfRealEigenvalues;

      Integer i;
      Integer l1;
      Integer l2;
      Integer k;
      Integer idx;
      Real eps=Modelica.Constants.eps;

      StateSpace ss=StateSpace(
          A=A,
          B=B,
          C=zeros(1, nx),
          D=zeros(1, size(B, 2)));

    algorithm
      //check controllability. To be omitted, since computationally expensive? It would be better to analyze the dimension of the nullspace of C
      assert(StateSpace.Internal.isControllableMIMO(ss), "Poles cannot be placed since system is not controllable");

      // sort eigenvalues to [real ev, complex ev(im>0), conj(complex ev(im>0))]
      (gammaSorted2,numberOfRealEigenvalues) :=
        Modelica_LinearSystems2.Internal.reorderZeros(gamma);
      gammaSorted := gammaSorted2;

      numberOfComplexPairs := integer((nx - numberOfRealEigenvalues)/2);
      for i in numberOfRealEigenvalues + 1:numberOfRealEigenvalues +
          numberOfComplexPairs loop
        gammaSorted[i] := if Im(gammaSorted2[2*i - numberOfRealEigenvalues - 1]) > 0 then
                gammaSorted2[2*i - numberOfRealEigenvalues - 1] else Complex.conj(
          gammaSorted2[2*i - numberOfRealEigenvalues - 1]);
        gammaSorted[i + numberOfComplexPairs] := Complex.conj(gammaSorted[i]);
      end for;

      S := fill(Complex(0), nx, size(B, 2)*(nx - numberOfComplexPairs));// set dimension of S
      (U0, Z, S, rankB) := Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.xBase(
                                                                             A, B, gammaSorted, numberOfComplexPairs);// calculation of S, bases of X
      X := Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.modifyX(
                                                                X,S,rankB,numberOfComplexPairs,maxSteps,IniX);// X modification, search optimal closed loop eigenvectors
      K := Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.calcK(
                                                             A,U0,Z,gammaSorted,X,numberOfRealEigenvalues);// calcualte feedbackmatrix K

    //    for i in 1:nx loop
    //      Lambda[i, i] := gammaSorted[i];
    //    end for;
    //   (sigmaB,Ur,Vr) := Modelica.Math.Matrices.singularValues(B);
    //   rankB := 0;
    //   i := size(sigmaB, 1);
    //   while i > 0 loop
    //     if sigmaB[i] > 1e-10 then
    //       rankB := i;
    //       i := 0;
    //     end if;
    //     i := i - 1;
    //   end while;
    //
    //  // ############  Calculation of S (bases of X)  begin
    //   Z := fill(0, size(B, 2), rankB);
    //   for l1 in 1:rankB loop
    //     for l2 in 1:size(B, 2) loop
    //       Z[l1, l2] := Vr[l2, l1]/sigmaB[l2];
    //     end for;
    //   end for;
    //   U0 := Ur[:, 1:rankB];
    //
    //    if nx>rankB then
    //   U1T := Complex(1)*transpose(Ur[:, rankB + 1:nx]);
    //   U1Tr := transpose(Ur[:, rankB + 1:nx]);
    //   condX2 := eps + 1;
    //
    //   if numberOfComplexPairs > 0 and 2*rankB - nx > 0 then
    //     Srr := Matrices.nullspace([U1Tr; U1Tr*A]);
    //   else
    //     Srr := fill(0, nx, 0);
    //   end if;
    //   Sr := Complex(1)*Srr;
    //
    //   //Computation of the nullspaces, i.e. the bases of the eigenvectors
    //   AC := Complex(-1)*A;
    //   for l1 in 1:nx - numberOfComplexPairs loop
    //     for l2 in 1:nx loop
    //       AC[l2, l2] := Complex(-A[l2, l2]) + gammaSorted[l1];
    //     end for;
    //
    //     C := matMul(U1T, AC);
    //
    //     S2 := if l1 > numberOfRealEigenvalues and 2*rankB - nx > 0 then [Matrices.C_nullspace([C; C_transpose(Sr)]),Sr] else Matrices.C_nullspace(C);
    //     for l2 in 1:nx loop
    //       for l3 in 1:rankB loop
    //         S[l2, rankB*(l1 - 1) + l3] := S2[l2, l3];
    //       end for;
    //     end for;
    //   end for;
    // // ############  Calculation of S (bases of X)  end

    // eigenvector modification

    //   if not IniX and rankB>1 then
    // // // initialization of X according to Byers
    // //   for l1 in 1:nx - numberOfComplexPairs loop
    // //     y := fill(Complex(0), nx);
    // //     for l2 in 1:rankB loop
    // // //      y := X[:, l1] + X[:, l1] + subS[l1].S[:, l2];
    // //       y := X[:, l1] + X[:, l1] + S[:, rankB*(l1-1)+l2];
    // //     end for;
    // //     y := y/Complex.Vectors.norm(y);
    // //     for l2 in 1:nx loop
    // //       X[l2, l1] := y[l2];
    // //     end for;
    // //   end for;
    // //   for l1 in 1:numberOfComplexPairs loop
    // //     for l2 in 1:nx loop
    // //       X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
    // //         Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
    // //     end for;
    // //   end for;
    //
    //  // initialization of X according to place.m
    //     for l1 in 1:nx - numberOfComplexPairs loop
    //       for l2 in 1:nx loop
    //         X[l2, l1] := S[l2, rankB*(l1 - 1) + 1];
    //       end for;
    //     end for;
    //     for l1 in 1:numberOfComplexPairs loop
    //       for l2 in 1:nx loop
    //         X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
    //           Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
    //       end for;
    //     end for;
    //   end if;

    //   if rankB==1 then //X=S
    //     for l1 in 1:nx - numberOfComplexPairs loop
    //       for l2 in 1:nx loop
    //         X[l2, l1] := S[l2, rankB*(l1 - 1) + 1];
    //       end for;
    //     end for;
    //     for l1 in 1:numberOfComplexPairs loop
    //       for l2 in 1:nx loop
    //         X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
    //           Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
    //       end for;
    //     end for;
    //     end if;

    //  if rankB>1 then
    //   k := 0;
    //   while (k < maxSteps) loop
    //     k := k + 1;
    //
    //     for l1 in 1:nx - numberOfComplexPairs loop
    //       if l1 == 1 then
    //         for l2 in 1:nx loop
    //           for l3 in 1:nx - 1 loop
    //             Xjj[l2, l3] := X[l2, l3 + 1];
    //           end for;
    //         end for;
    //       else
    //         for l2 in 1:nx loop
    //           for l3 in 1:l1 - 1 loop
    //             Xjj[l2, l3] := X[l2, l3];
    //           end for;
    //           for l3 in l1:nx - 1 loop
    //             Xjj[l2, l3] := X[l2, l3 + 1];
    //           end for;
    //         end for;
    //       end if;
    //
    //       QX := Modelica_LinearSystems2.Math.Matrices.C_QR(Xjj);
    //
    //       ST := C_transpose(S[:, rankB*(l1 - 1) + 1:rankB*l1]);
    //       y := matVecMul(ST, QX[:, nx]);
    //
    //       norm_y := Complex.Vectors.norm(y);
    //       y := matVecMul(S[:, rankB*(l1 - 1) + 1:rankB*l1], y)/norm_y;
    //
    //       if l1 > numberOfRealEigenvalues and Complex.'abs'(
    //           Complex.Vectors.multiply(y, Complex.conj(y))) > 0.9 then
    //         idx := 1 + rem(k, rankB - size(Sr, 2));
    //         y := (y + S[:, (l1 - 1)*rankB + idx])/sqrt(2);
    //       end if;
    //
    //       for l2 in 1:nx loop
    //         X[l2, l1] := y[l2];
    //       end for;
    //
    //       if l1 > numberOfRealEigenvalues then
    //         for l2 in 1:nx loop
    //           X[l2, l1 + numberOfComplexPairs] := Complex.conj(y[l2]);
    //         end for;
    //       end if;
    //     end for;
    //     condX2 := Complex.Matrices.conditionNumber(X);
    //   end while;
    // end if;
    // Computation of the feedback matrix K
    //    XC := C_transpose(X);
    //    XC2 := C_transpose(matMul(X, Lambda));
    //    MM := Modelica_LinearSystems2.Math.Matrices.C_solve2(XC, XC2);
    //    M := Re(MM);
    //    M := transpose(M);
    //
    //    for l2 in 1:nx loop
    //      for l3 in 1:nx loop
    //        M[l2, l3] := M[l2, l3] - A[l2, l3];
    //      end for;
    //    end for;
    //  else//nx>rankB
    //      X:=fill(Complex(0),nx,nx);
    //     for i in 1:numberOfRealEigenvalues loop
    //       M[i,i]:=Re(gammaSorted[i]);
    //       X[i,i] := Complex(1);
    //     end for;
    //     for i in 1:numberOfComplexPairs loop
    //           M[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i] := Im(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
    //           M[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i-1] := -Im(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
    //           M[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i-1] := Re(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
    //           M[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i] := Re(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
    //           Modelica_LinearSystems2.Math.Matrices.printMatrix(M,6,"M");
    //           X[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i-1] := Complex(0.5);
    //           X[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i] := Complex(0,0.5);
    //           X[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i] := Complex(0,-0.5);
    //           X[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i-1] := Complex(0.5);
    //     end for;
    //     M:=M-A;

    // end if;//nx>rankB
    //    K := -Z*transpose(U0)*M;

    end assignPolesMI_rob;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.assignPolesMI_rob2

Modified KNV algorithm. Works like MATLAB's place.m

### Information

Extends from Modelica.Icons.Function (Icon for a function).

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | A[:, size(A, 1)] |  | system matrix
Real | B[size(A, 1), :] |  | control input matrix
Complex | gamma[size(A, 1)] |  |
Boolean | IniX | false | Initial values of X are provided
Complex | Xini[size(A, 1), size(A, 1)] | fill(Complex(0), size(A, 1),... | Initial values of the eigenvectors X

### Outputs

Type | Name | Description
--- | --- | ---
Real | K[size(B, 2), size(A, 1)] | feedback matrix
Complex | X[size(A, 1), size(A, 2)] | eigen vectors of the closed loop system

### Modelica definition

    function assignPolesMI_rob2
      "Modified KNV algorithm. Works like MATLAB's place.m"
      extends Modelica.Icons.Function;

      import Modelica;
      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.StateSpace;
      import Modelica_LinearSystems2.Math.Matrices;
      import Modelica_LinearSystems2.Math.Complex;
      import matMul = Modelica_LinearSystems2.Math.Complex.Matrices.matMatMul;
      import Modelica_LinearSystems2.Math.Complex.Matrices.matVecMul;
      import Modelica_LinearSystems2.Math.Complex.Internal.C_transpose;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;
      import Modelica.Utilities.Streams.print;

      input Real A[:,size(A, 1)] "system matrix";
      input Real B[size(A, 1),:] "control input matrix";
      input Complex gamma[size(A, 1)];
      input Boolean IniX=false "Initial values of X are provided";
      input Complex Xini[size(A, 1),size(A, 1)]=fill(Complex(0), size(A, 1), size(A, 1))
        "Initial values of the eigenvectors X";

      output Real K[size(B, 2),size(A, 1)] "feedback matrix";
      output Complex X[size(A, 1),size(A, 2)]=Xini
        "eigen vectors of the closed loop system";
    protected
      Real U0[:,:];
      Real Z[:,:];
      Complex S[:,:];
      Integer maxSteps=5;
      Integer numberOfComplexPairs;
      Integer rankB;

      Complex AC[size(A, 1),size(A, 2)]=Complex(1)*A;
      Complex Lambda[size(A, 1),size(A, 1)];
      Real Ur[:,:];
      Real Vr[:,:];

      Complex U1T[:,:];
      Complex Xjj[size(A, 1),size(A, 1)];
      Complex XC[size(A, 1),size(A, 2)];
      Complex XC2[size(A, 1),size(A, 2)];
      Complex MM[size(A, 1),size(A, 1)];
      Real M[size(A, 1),size(A, 1)];
      Complex KC[:,:];
      Complex QX[:,:];
      Complex C[:,:];
      Complex Cc[:,:];
      Complex Sr[:,:];
      Real Srr[:,:];
      Real U1Tr[:,:];
      Complex ST[:,:];

      Complex S2[size(A, 1),size(B, 2)];

      Complex gammaSorted[size(gamma, 1)];
      Complex gammaSorted2[size(gamma, 1)];
      Real sigmaB[:];
    //  Complex ev[size(A, 1)];

      Complex y[:];
      Real condX2;
      Real norm_y;

      Integer nx=size(A, 1);
      Integer numberOfRealEigenvalues;

      Integer i;
      Integer l1;
      Integer l2;
      Integer k;
      Integer idx;
      Real eps=Modelica.Constants.eps;

    //  Modelica_LinearSystems2.StateSpace.Internal.assignPolesMI_rob.subSpace subS[size(gamma,1)];

    //  Complex a;
    //  Integer cnt;
      StateSpace ss=StateSpace(
          A=A,
          B=B,
          C=zeros(1, nx),
          D=zeros(1, size(B, 2)));

       Complex XX[size(A, 1),size(A, 2)];

    algorithm
      //check controllability
      assert(StateSpace.Analysis.isControllable(ss), "Poles cannot be placed since system is not controllable");

      // sort eigenvalues to [real ev, complex ev(im>0), conj(complex ev(im>0))]
      (gammaSorted2,numberOfRealEigenvalues) :=
        Modelica_LinearSystems2.Internal.reorderZeros(gamma);
      gammaSorted := gammaSorted2;
      numberOfComplexPairs := integer((nx - numberOfRealEigenvalues)/2);
      for i in numberOfRealEigenvalues + 1:numberOfRealEigenvalues +
          numberOfComplexPairs loop
        gammaSorted[i] := if Im(gammaSorted2[2*i - numberOfRealEigenvalues - 1]) > 0 then
                gammaSorted2[2*i - numberOfRealEigenvalues - 1] else Complex.conj(
          gammaSorted2[2*i - numberOfRealEigenvalues - 1]);
        gammaSorted[i + numberOfComplexPairs] := Complex.conj(gammaSorted[i]);
      end for;

      for i in 1:nx loop
        Lambda[i, i] := gammaSorted[i];
      end for;

       (sigmaB,Ur,Vr) := Modelica.Math.Matrices.singularValues(B);
       rankB := 0;
       i := size(sigmaB, 1);
       while i > 0 loop
         if sigmaB[i] > 1e-10 then
           rankB := i;
           i := 0;
         end if;
         i := i - 1;
       end while;

      // ############  Calculation of S (bases of X)  begin
       Z := fill(0, size(B, 2), rankB);
       for l1 in 1:rankB loop
         for l2 in 1:size(B, 2) loop
           Z[l1, l2] := Vr[l2, l1]/sigmaB[l2];
         end for;
       end for;
       U0 := Ur[:, 1:rankB];

     if nx>size(B,2) then

       U1T := Complex(1)*transpose(Ur[:, rankB + 1:nx]);
       U1Tr := transpose(Ur[:, rankB + 1:nx]);
       condX2 := eps + 1;

      S := fill(Complex(0), nx, size(B, 2)*(nx - numberOfComplexPairs));

       if numberOfComplexPairs > 0 and 2*rankB - nx > 0 then
         Srr := Matrices.nullspace([U1Tr; U1Tr*A]);
       else
         Srr := fill(0, nx, 0);
       end if;
       Sr := Complex(1)*Srr;

       //Computation of the nullspaces, i.e. the bases of the eigenvectors
       AC := Complex(-1)*A;
       for l1 in 1:nx - numberOfComplexPairs loop
         for l2 in 1:nx loop
           AC[l2, l2] := Complex(-A[l2, l2]) + gammaSorted[l1];
         end for;

         C := matMul(U1T, AC);

         S2 := if l1 > numberOfRealEigenvalues and 2*rankB - nx > 0 then [Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_nullspace(
                                                                                               [C; C_transpose(Sr)]),Sr] else Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_nullspace(
                                                                                                        C);
         for l2 in 1:nx loop
           for l3 in 1:rankB loop
             S[l2, rankB*(l1 - 1) + l3] := S2[l2, l3];
           end for;
         end for;
       end for;
     // ############  Calculation of S (bases of X)  end
    // (U0, Z, S, rankB) := Modelica_LinearSystems2.StateSpace.Internal.xBase(A, B, gammaSorted, numberOfComplexPairs);

      if not IniX and rankB>1 then
    // // initialization of X according to Byers
    //   for l1 in 1:nx - numberOfComplexPairs loop
    //     y := fill(Complex(0), nx);
    //     for l2 in 1:rankB loop
    // //      y := X[:, l1] + X[:, l1] + subS[l1].S[:, l2];
    //       y := X[:, l1] + X[:, l1] + S[:, rankB*(l1-1)+l2];
    //     end for;
    //     y := y/Complex.Vectors.norm(y);
    //     for l2 in 1:nx loop
    //       X[l2, l1] := y[l2];
    //     end for;
    //   end for;
    //   for l1 in 1:numberOfComplexPairs loop
    //     for l2 in 1:nx loop
    //       X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
    //         Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
    //     end for;
    //   end for;

     // initialization of X according to place.m
        for l1 in 1:nx - numberOfComplexPairs loop
          for l2 in 1:nx loop
            X[l2, l1] := S[l2, rankB*(l1 - 1) + 1];
          end for;
        end for;
        for l1 in 1:numberOfComplexPairs loop
          for l2 in 1:nx loop
            X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
              Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
          end for;
        end for;
      end if;

      if rankB==1 then //X=S
        for l1 in 1:nx - numberOfComplexPairs loop
          for l2 in 1:nx loop
            X[l2, l1] := S[l2, rankB*(l1 - 1) + 1];
          end for;
        end for;
        for l1 in 1:numberOfComplexPairs loop
          for l2 in 1:nx loop
            X[l2, numberOfRealEigenvalues + numberOfComplexPairs + l1] :=
              Complex.conj(X[l2, numberOfRealEigenvalues + l1]);
          end for;
        end for;
      end if;

    // eigenvector modification
    //  X := Modelica_LinearSystems2.StateSpace.Internal.modifyX( X,S,rankB,numberOfComplexPairs,maxSteps,IniX);

      if rankB>1 then
       k := 0;
       while (k < maxSteps) loop
         k := k + 1;

         for l1 in 1:nx - numberOfComplexPairs loop
           if l1 == 1 then
             for l2 in 1:nx loop
               for l3 in 1:nx - 1 loop
                 Xjj[l2, l3] := X[l2, l3 + 1];
               end for;
             end for;
           else
             for l2 in 1:nx loop
               for l3 in 1:l1 - 1 loop
                 Xjj[l2, l3] := X[l2, l3];
               end for;
               for l3 in l1:nx - 1 loop
                 Xjj[l2, l3] := X[l2, l3 + 1];
               end for;
             end for;
           end if;

           QX := Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_QR(
                                                            Xjj);

           ST := C_transpose(S[:, rankB*(l1 - 1) + 1:rankB*l1]);
           y := matVecMul(ST, QX[:, nx]);

           norm_y := Complex.Vectors.norm(y);
           y := matVecMul(S[:, rankB*(l1 - 1) + 1:rankB*l1], y)/norm_y;

           if l1 > numberOfRealEigenvalues and Complex.'abs'(
               Complex.Vectors.multiply(y, Complex.conj(y))) > 0.9 then
             idx := 1 + rem(k, rankB - size(Sr, 2));
             y := (y + S[:, (l1 - 1)*rankB + idx])/sqrt(2);
           end if;

           for l2 in 1:nx loop
             X[l2, l1] := y[l2];
           end for;

           if l1 > numberOfRealEigenvalues then
             for l2 in 1:nx loop
               X[l2, l1 + numberOfComplexPairs] := Complex.conj(y[l2]);
             end for;
           end if;
         end for;
         condX2 := Modelica_LinearSystems2.WorkInProgress.Math.Complex.Matrices.conditionNumber(
                                                    X);
       end while;
       end if;//rankB>1

    // Computation of the feedback matrix K
       XC := C_transpose(X);
       XC2 := C_transpose(matMul(X, Lambda));
       MM := Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_solve2(
                                                            XC, XC2);
       M := Re(MM);
       M := transpose(M);

       for l2 in 1:nx loop
         for l3 in 1:nx loop
           M[l2, l3] := M[l2, l3] - A[l2, l3];
         end for;
         end for;

       else
     gammaSorted2 :=  Modelica_LinearSystems2.Internal.reorderZeros(gamma);
        for i in 1:nx loop
        Lambda[i, i] := gammaSorted2[i];
      end for;
      X:=fill(Complex(0),nx,nx);

       for i in 1:numberOfRealEigenvalues loop
         M[i,i]:=Re(gammaSorted[i]);
         X[i,i] := Complex(1);
       end for;
       for i in 1:numberOfComplexPairs loop
             M[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i] := -Im(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
             M[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i-1] := Im(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
             M[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i-1] := Re(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
             M[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i] := Re(gammaSorted[numberOfRealEigenvalues + 2*i - 1]);
             X[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i-1] := Complex(0.5);
             X[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i] := Complex(0,0.5);
             X[numberOfRealEigenvalues+2*i-1,numberOfRealEigenvalues+2*i] := Complex(0,-0.5);
             X[numberOfRealEigenvalues+2*i,numberOfRealEigenvalues+2*i-1] := Complex(0.5);
       end for;
             Modelica_LinearSystems2.Math.Complex.Matrices.print(X,6,"X");
             Modelica_LinearSystems2.Math.Matrices.printMatrix(M,6,"M");
             MM := Complex(1)*M;
             XX := matMul(MM,X);
             XX := Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_solve2(
                                                                  X, XX);
             Modelica_LinearSystems2.Math.Complex.Matrices.print(XX,6,"XX");
             M:=M-A;
             Modelica_LinearSystems2.Math.Matrices.printMatrix(M,6,"M-A");

       end if;

       K := -Z*transpose(U0)*M;
    //  evX := X;
    //K:=Modelica_LinearSystems2.StateSpace.Internal.calcK(A,U0,Z,gammaSorted,X);

    end assignPolesMI_rob2;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.assignPolesSI_rq

RQ implementation of a recursiv single-input pole assignment problem

### Information

Extends from Modelica.Icons.Function (Icon for a function).

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
StateSpace | ss |  |
Complex | gamma[size(ss.A, 1)] |  |

### Outputs

Type | Name | Description
--- | --- | ---
Real | K[1, size(ss.A, 1)] | feedback matrix
Real | S[size(ss.A, 1), size(ss.A, 1)] | Closed loop System matrix
Complex | po[size(ss.A, 1)] | poles of the closed loop system

### Modelica definition

    function assignPolesSI_rq
      "RQ implementation of a recursiv single-input pole assignment problem"
      extends Modelica.Icons.Function;

      import Modelica;
      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.StateSpace;
      import Modelica_LinearSystems2.Math.Matrices;
      import Modelica_LinearSystems2.Math.Complex;
      import matMul = Modelica_LinearSystems2.Math.Complex.Matrices.matMatMul;
      import Modelica_LinearSystems2.Math.Complex.Matrices.matVecMul;
      import Modelica_LinearSystems2.Math.Complex.Internal.C_transpose;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;
      import Modelica.Utilities.Streams.print;

      input StateSpace ss;
      input Complex gamma[size(ss.A, 1)];

      output Real K[1,size(ss.A, 1)] "feedback matrix";
      output Real S[size(ss.A, 1),size(ss.A, 1)] "Closed loop System matrix";
      output Complex po[size(ss.A, 1)] "poles of the closed loop system";
    protected
      Boolean isCntrbl "is set to true if the system is controllable";
      Modelica_LinearSystems2.Internal.StateSpaceR ssr "system in Hessenberg form";

      Integer nx=size(ss.A, 1);
      Complex H[size(ss.A, 1),size(ss.A, 1)];
      Complex Q[size(ss.A, 1),size(ss.A, 1)];
      Complex R[size(ss.A, 1),size(ss.A, 1)];
      Complex Qi[:,:];
      Complex Ri[:,:];
      Complex Hi[:,:];
      Complex Hlr[:,:];
      Complex Qn[size(ss.A, 1),size(ss.A, 1)];
      Complex rho;

      Real alpha=1.0;
      Real beta;
      Integer i;
      Integer ni;

      Real P[:,:];

      Complex ev[:];
      Complex RQ[:,:];
      Complex tau[:];

    algorithm
      //reduction to controller-Hessenberg form
      (isCntrbl,ssr,P) := StateSpace.Internal.staircaseSVD(ss);

      assert(isCntrbl, "Poles cannot be placed since system is not controllable");

      H := Complex(1)*ssr.A;

      if nx == 1 then
        K[1, 1] := (ss.A[1, 1] - Re(gamma[1]))/ss.B[1,1];
      else
        beta := ssr.B[1,1];
        for i in 2:nx loop
          alpha := alpha*Re(H[i, i - 1]);
        end for;

        for i in 1:nx loop
          H[i, i] := H[i, i] - gamma[1];
        end for;

        (R,Q) := Modelica_LinearSystems2.WorkInProgress.Math.Matrices.C_RQ(
                               H);
        H := matMul(Q, R);

        for i in 1:nx loop
          H[i, i] := H[i, i] + gamma[1];
        end for;

        rho := R[nx, nx];
        Hi := H;

        for i in 2:nx loop
          ni := size(Hi, 1);
          for ii in 1:ni loop
            Hi[ii, ii] := Hi[ii, ii] - gamma[i];
          end for;

    //       (Ri,Qi) :=  Matrices.C_RQ(Hi);
    //       Hlr := Qi*Ri;
    // replaced by :########
          (RQ,tau) := Modelica_LinearSystems2.WorkInProgress.Math.LAPACK.zgerq2(
                                             Hi);
          Ri := fill(Complex(0),ni,ni);
          for ii in 1:ni loop
            for iii in ii:ni loop
              Ri[ii,iii] := RQ[ii,iii];
            end for;
          end for;
          Qi := Modelica_LinearSystems2.WorkInProgress.Math.LAPACK.zungrq(
                                       RQ,tau);
          Hlr := Modelica_LinearSystems2.WorkInProgress.Math.LAPACK.zunmrq(
                                        Ri,RQ,tau,true);
    // ##########

          for ii in 1:ni loop
            Hlr[ii, ii] := Hlr[ii, ii] + gamma[i];
          end for;
          Hi := Hlr[2:ni, 2:ni];

          if i > 2 then
            Qn := fill(Complex(0), nx, nx);
            for ii in 1:i - 2 loop
              Qn[ii, ii] := Complex(1);
            end for;
            for ii in 1:nx-i+2 loop
              for iii in 1:nx-i+2 loop
                Qn[ii+i-2, iii+i-2] := Qi[ii, iii];
              end for;
            end for;
            Q := matMul(Qn,Q);
          else
            Q := matMul(Qi,Q);
          end if;

          rho := rho*Ri[nx + 2 - i, nx + 2 - i];
        end for;

        K[1, :] := Q[nx, :].re;
        K := (rho.re/alpha/beta)*K*P;
        end if;
        S := ss.A-ss.B*K;
        po :=  Complex.eigenValues(S);
        Complex.Vectors.print("ev",ev);
    end assignPolesSI_rq;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.xBase

Compute the eigenvector bases according to Kautsky algorithm

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | A[:, size(A, 1)] |  | Real square system matrix
Real | B[size(A, 1), :] |  | Real input matrix
Complex | gamma[size(A, 1)] |  | Assigned complex eigenvalues
Integer | ncp |  | Number of complex pairs of eigenvalues

### Outputs

Type | Name | Description
--- | --- | ---
Real | U0[size(A, 1), size(B, 2)] | U0 and Z are the decompositions of B
Real | Z[size(B, 2), size(B, 2)] | Z and U0 are the decompositions of B
Complex | S[size(A, 1), (size(A, 1) - ncp)*size(B, 2)] | Eigenvector bases
Integer | rankB |

### Modelica definition

    function xBase
      "Compute the eigenvector bases according to Kautsky algorithm"
      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.Math.Complex;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;

      input Real A[:,size(A,1)] "Real square system matrix";
      input Real B[size(A,1),:] "Real input matrix";
      input Complex gamma[size(A,1)] "Assigned complex eigenvalues";
      input Integer ncp "Number of complex pairs of eigenvalues";

      output Real U0[size(A, 1),size(B, 2)] "U0 and Z are the decompositions of B";
      output Real Z[size(B,2),size(B,2)] "Z and U0 are the decompositions of B";
      output Complex S[size(A,1),(size(A,1)-ncp)*size(B,2)] "Eigenvector bases";
      output Integer rankB;

    protected
       Complex j=Modelica_LinearSystems2.Math.Complex.j();
       Integer n=size(A,1);
       Integer m=size(B,2);

       Real gamma_real[n]=Re(gamma) "Eigenvalue vector, real part";
       Real gamma_imag[n]=Im(gamma) "Eigenvalue vector, imaginary part";
       Real S_real[n,m*(n-ncp)] "Eigenvector bases, real part";
       Real S_imag[n,m*(n-ncp)] "Eigenvector bases, imaginary part";
      Integer i;
      Integer ii;

    algorithm
      (U0, Z, S_real, S_imag, rankB) :=Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_xBase(
                                                                                                 A, B, gamma_real, gamma_imag, ncp);
      assert(m==rankB,"Input matrix B must have full column rank");
      for i in 1:n loop
        for ii in 1:(n-ncp)*m loop
          S[i,ii] := Complex(S_real[i,ii],S_imag[i,ii]);
        end for;
      end for;
    end xBase;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.calcK

Computes the feedback matrix from the assigned eigenvalues, closed loop eigenvectors and the B matrix factorization

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | A[:, size(A, 1)] |  | Real square system matrix
Real | U0[size(A, 1), :] |  | U0 and Z are the decompositions of B
Real | Z[size(U0, 2), size(U0, 2)] |  | Z and U0 are the decompositions of B
Complex | gamma[size(A, 1)] |  | Assigned complex eigenvalues
Complex | X[size(A, 1), size(A, 1)] |  | Closed loop eigenvectors
Integer | nre |  | number of real eigenvalues

### Outputs

Type | Name | Description
--- | --- | ---
Real | K[size(U0, 2), size(A, 1)] | Feedback matrix

### Modelica definition

    function calcK
      "Computes the feedback matrix from the assigned eigenvalues, closed loop eigenvectors and the B matrix factorization"
      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.Math.Complex;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;

      input Real A[:,size(A, 1)] "Real square system matrix";
      input Real U0[size(A, 1),:] "U0 and Z are the decompositions of B";
      input Real Z[size(U0, 2),size(U0, 2)] "Z and U0 are the decompositions of B";
      input Complex gamma[size(A, 1)] "Assigned complex eigenvalues";
      input Complex X[size(A, 1),size(A, 1)] "Closed loop eigenvectors";
      input Integer nre "number of real eigenvalues";

      output Real K[size(U0, 2),size(A, 1)] "Feedback matrix";

    protected
      Integer n=size(A, 1);
      Integer m=size(U0, 2);

      Real gamma_real[n]=Re(gamma) "Eigenvalue vector, real part";
      Real gamma_imag[n]=Im(gamma) "Eigenvalue vector, imaginary part";
      Real X_real[n,n]=Re(X) "Eigenvectors, real part";
      Real X_imag[n,n]=Im(X) "Eigenvectors, imaginary part";

    algorithm
      K := Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_calcK(
                                                                     A, U0, Z, gamma_real, gamma_imag, X_real,X_imag, nre);
    end calcK;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.modifyX

Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Complex | X[:, size(X, 1)] |  | Complex eigenvector matrix
Complex | S[size(X, 1), :] |  | Complex eigenvector matrix
Integer | m |  | Rank of the system input matrix B; S_real and S_imag must have n*m columns
Integer | ncp |  | number of complex pairs
Integer | steps |  | Number of iterations
Boolean | IniX | false | Initial values of X are provided

### Outputs

Type | Name | Description
--- | --- | ---
Complex | Xm[size(X, 1), size(X, 2)] |

### Modelica definition

    function modifyX
      "Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm"

      import Modelica_LinearSystems2;
      import Modelica_LinearSystems2.Math.Complex;
      import Re = Modelica_LinearSystems2.Math.Complex.real;
      import Im = Modelica_LinearSystems2.Math.Complex.imag;

      input Complex X[:,size(X,1)] "Complex eigenvector matrix";
      input Complex S[size(X,1),:] "Complex eigenvector matrix";
      input Integer m
        "Rank of the system input matrix B; S_real and S_imag must have n*m columns";
      input Integer ncp "number of complex pairs";
      input Integer steps "Number of iterations";
      input Boolean IniX=false "Initial values of X are provided";

      output Complex Xm[size(X, 1),size(X, 2)];

    protected
       Complex j=Modelica_LinearSystems2.Math.Complex.j();
       Integer n=size(X,1);
       Real X_real[n,n]=Re(X) "Eigenvector matrix, real part";
       Real X_imag[n,n]=Im(X) "Eigenvector matrix, imaginary part";
       Real S_real[n,m*(n-ncp)]=Re(S) "Eigenvector bases, real part";
       Real S_imag[n,m*(n-ncp)]=Im(S) "Eigenvector bases, imaginary part";

       Real Xm_real[n,n];
       Real Xm_imag[n,n];

      Integer i;
      Integer ii;

    algorithm
      (Xm_real, Xm_imag) :=Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_modifyX(
                                                                                        X_real, X_imag, n, S_real, S_imag, m, ncp, steps, IniX);
      for i in 1:n loop
        for ii in 1:n loop
          Xm[i,ii] := Complex(Xm_real[i,ii],Xm_imag[i,ii]);
        end for;
      end for;
    end modifyX;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_calcK

Computes the feedback matrix from the assigned eigenvalues, closed loop eigenvectors and the B matrix factorization

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | A[:, size(A, 1)] |  | Real square system matrix
Real | U0[size(A, 1), :] |  | U0 and Z are the decompositions of B
Real | Z[size(U0, 2), size(U0, 2)] |  | Z and U0 are the decompositions of B
Real | gamma_real[size(A, 1)] |  | Eigenvalue vector, real part
Real | gamma_imag[size(A, 1)] |  | Eigenvalue vector, imaginary part
Real | X_real[n, n] |  | Eigenvectors, real part
Real | X_imag[n, n] |  | Eigenvectors, imaginary part
Integer | nre |  | Number of real eigenvalues

### Outputs

Type | Name | Description
--- | --- | ---
Real | K[size(U0, 2), size(A, 1)] | Feedback matrix

### Modelica definition

    function wrapper_calcK
      "Computes the feedback matrix from the assigned eigenvalues, closed loop eigenvectors and the B matrix factorization"

      input Real A[:,size(A, 1)] "Real square system matrix";
      input Real U0[size(A, 1),:] "U0 and Z are the decompositions of B";
      input Real Z[size(U0, 2),size(U0, 2)] "Z and U0 are the decompositions of B";
      input Real gamma_real[size(A,1)] "Eigenvalue vector, real part";
      input Real gamma_imag[size(A,1)] "Eigenvalue vector, imaginary part";
      input Real X_real[n,n] "Eigenvectors, real part";
      input Real X_imag[n,n] "Eigenvectors, imaginary part";
      input Integer nre "Number of real eigenvalues";

      output Real K[size(U0,2), size(A,1)] "Feedback matrix";

    protected
      Integer n=size(A,1);
      Integer m=size(U0,2);

    external"FORTRAN 77" c_inter_calcK(n, m, A, U0, Z, gamma_real, gamma_imag, X_real, X_imag, nre, K);

    end wrapper_calcK;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_modifyX

Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | X_real[:, size(X_real, 1)] |  | Eigenvector matrix, real part
Real | X_imag[size(X_real, 1), size(X_real, 2)] |  | Eigenvector matrix, imaginary part
Integer | n |  | Order of X
Real | S_real[size(X_real, 1), :] |  | Eigenvector bases, real part
Real | S_imag[size(S_real, 1), size(S_real, 2)] |  | Eigenvector bases, imaginary part
Integer | m |  | Rank of the system input matrix B; S_real and S_imag must have n*m columns
Integer | ncp |  | number of complex pairs
Integer | steps |  | Number of iterations
Boolean | IniX | false | Initial values of X are provided

### Outputs

Type | Name | Description
--- | --- | ---
Real | Xm_real[size(X_real, 1), size(X_real, 2)] |
Real | Xm_imag[size(X_imag, 1), size(X_imag, 2)] |

### Modelica definition

    function wrapper_modifyX
      "Contains a C sub routine of robust pole assignment to modify the eigenvector matrix X according to Kautsky algorithm"
      input Real X_real[:,size(X_real, 1)] "Eigenvector matrix, real part";
      input Real X_imag[size(X_real, 1),size(X_real, 2)]
        "Eigenvector matrix, imaginary part";
      input Integer n "Order of X";
      input Real S_real[size(X_real, 1),:] "Eigenvector bases, real part";
      input Real S_imag[size(S_real, 1),size(S_real, 2)]
        "Eigenvector bases, imaginary part";
      input Integer m
        "Rank of the system input matrix B; S_real and S_imag must have n*m columns";
      input Integer ncp "number of complex pairs";
      input Integer steps "Number of iterations";
      input Boolean IniX=false "Initial values of X are provided";

      output Real Xm_real[size(X_real, 1),size(X_real, 2)]=X_real;
      output Real Xm_imag[size(X_imag, 1),size(X_imag, 2)]=X_imag;

    protected
      Integer inix=if IniX then 1 else 0;

    external"FORTRAN 77" c_inter_modifyX2(
        Xm_real,
        Xm_imag,
        n,
        S_real,
        S_imag,
        m,
        ncp,
        steps,
        inix);

    end wrapper_modifyX;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.wrapper_xBase

Compute the eigenvector bases according to Kautsky algorithm

### Inputs

Type | Name | Default | Description
--- | --- | --- | ---
Real | A[:, size(A, 1)] |  | Real square system matrix
Real | B[size(A, 1), :] |  | Real input matrix
Real | gamma_real[size(A, 1)] |  | Eigenvalue vector, real part
Real | gamma_imag[size(A, 1)] |  | Eigenvalue vector, imaginary part
Integer | ncp |  | Number of complex pairs of eigenvalues

### Outputs

Type | Name | Description
--- | --- | ---
Real | U0[size(A, 1), size(B, 2)] | U0
Real | Z[size(B, 2), size(B, 2)] | Z
Real | S_real[size(A, 1), size(B, 2)*(size(A, 1) - ncp)] | Eigenvector bases, real part
Real | S_imag[size(A, 1), size(B, 2)*(size(A, 1) - ncp)] | Eigenvector bases, imaginary part
Integer | rankB | Rank of matrix B

### Modelica definition

    function wrapper_xBase
      "Compute the eigenvector bases according to Kautsky algorithm"

      input Real A[:,size(A, 1)] "Real square system matrix";
      input Real B[size(A,1),:] "Real input matrix";
      input Real gamma_real[size(A,1)] "Eigenvalue vector, real part";
      input Real gamma_imag[size(A,1)] "Eigenvalue vector, imaginary part";
      input Integer ncp "Number of complex pairs of eigenvalues";

      output Real U0[size(A,1),size(B,2)] "U0";
      output Real Z[size(B,2),size(B,2)] "Z";
      output Real S_real[size(A,1),size(B,2)*(size(A,1)-ncp)]
        "Eigenvector bases, real part";
      output Real S_imag[size(A,1),size(B,2)*(size(A,1)-ncp)]
        "Eigenvector bases, imaginary part";
      output Integer rankB "Rank of matrix B";

    protected
      Integer n=size(A,1);
      Integer m=size(B,2);

    external"FORTRAN 77" c_inter_xBase(A, B, n, m, gamma_real, gamma_imag, ncp, U0, Z, S_real, S_imag, rankB);

    end wrapper_xBase;

## Modelica_LinearSystems2.WorkInProgress.StateSpace.Internal.assignPolesMI_rob_old.subSpace

### Information

Extends from Modelica.Icons.Record (Icon for a record).

### Modelica definition

    encapsulated record subSpace
      import Modelica;
      import Modelica_LinearSystems2;
      extends Modelica.Icons.Record;
      Modelica_LinearSystems2.Math.Complex S[:,:];
    end subSpace;