Tfm2des.java

/*
 * Created on 2010/09/29
 * Copyright (C) 2010 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoublePolynomialMatrix;
import org.mklab.nfc.matrix.DoubleRationalPolynomialMatrix;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;


/**
 * 
 * 伝達関数行列(有理多項式行列)からディスクリプタ表現に変換するクラスです。 <p>Transfer function　matrix to descriptor-state-space conversion</p>
 * 
 * @author esumi
 * @version $Revision$, 2010/09/29
 */
public class Tfm2des {

  /**
   * 多入力多出力システム(非プロパー)
   * 
   * <pre><code>
   * 
   *        NUM(s) 
   * G(s) = -------- 
   *        den(s)
   * 
   * </code></pre>
   * 
   * の状態空間表現
   * 
   * <pre><code>
   * 
   *  . 
   * Ex = Ax + Bu 
   *  y = Cx + Du
   * 
   * </code></pre>
   * 
   * を求めます。 係数行列 <code>A,B,C,D</code> は、可制御正準形で返されます。
   * 
   * @param transferFunctionMatrix 伝達関数行列
   * @return Descriptor DoubleMatrix List{A,B,C,D,E} ディスクリプター表現(descriptor state space representation)
   */
  public static List<DoubleMatrix> tfm2des(DoubleRationalPolynomialMatrix transferFunctionMatrix) {
    final DoublePolynomialMatrix pol = transferFunctionMatrix.getQuotientElementWise();
    final DoubleRationalPolynomialMatrix pro = transferFunctionMatrix.subtract(new DoubleRationalPolynomialMatrix(pol));

    final List<DoubleMatrix> sp = Tfm2ss.tfm2ss(pro);

    final int columnSize = transferFunctionMatrix.getColumnSize();
    final int rowSize = transferFunctionMatrix.getRowSize();

    final DoubleRationalPolynomialMatrix hornerPol = new DoubleRationalPolynomialMatrix(rowSize, columnSize);
    final double denCoefficient = 1.0;

    for (int column = 1; column <= columnSize; column++) {
      for (int row = 1; row <= rowSize; row++) {
        DoublePolynomial numPolynomial = new DoublePolynomial(pol.getElement(row, column).getCoefficients().flipLeftRight().divide(-1));
        int numPolynomialDegree = pol.getElement(row, column).getDegree() + 1;
        double[] denPolynomialCoefficient = new double[numPolynomialDegree + 1];
        denPolynomialCoefficient[numPolynomialDegree] = denCoefficient;
        DoublePolynomial denPolynomial = new DoublePolynomial(denPolynomialCoefficient);
        hornerPol.setElement(row, column, new DoubleRationalPolynomial(numPolynomial, denPolynomial));
      }
    }

    final List<DoubleMatrix> spol = Tfm2ss.tfm2ss(hornerPol);

    final int spRowSize = sp.get(0).getRowSize();
    final int spColumnSize = sp.get(0).getColumnSize();
    final int spolRowSize = spol.get(0).getRowSize();
    final int spolColumnSize = spol.get(0).getColumnSize();

    final int inputDegree = transferFunctionMatrix.getColumnSize();
    final int outputDegree = transferFunctionMatrix.getRowSize();

    final DoubleMatrix A = new DoubleMatrix(spRowSize + spolRowSize, spColumnSize + spolColumnSize);
    final DoubleMatrix B = new DoubleMatrix(spRowSize + spolRowSize, inputDegree);
    final DoubleMatrix C = new DoubleMatrix(outputDegree, spColumnSize + spolColumnSize);
    final DoubleMatrix D = new DoubleMatrix(outputDegree, inputDegree);
    final DoubleMatrix E = new DoubleMatrix(spRowSize + spolRowSize, spColumnSize + spolColumnSize);

    final DoubleMatrix leftLowerMatrix = new DoubleMatrix(spolRowSize, spColumnSize);
    final DoubleMatrix upperRightMatrix = new DoubleMatrix(spRowSize, spolColumnSize);

    A.setSubMatrix(1, spRowSize, 1, spColumnSize, sp.get(0));
    A.setSubMatrix(spRowSize + 1, spRowSize + spolRowSize, 1, spColumnSize, leftLowerMatrix);
    A.setSubMatrix(1, spRowSize, spColumnSize + 1, spColumnSize + spolColumnSize, upperRightMatrix);
    A.setSubMatrix(spRowSize + 1, spRowSize + spolRowSize, spColumnSize + 1, spColumnSize + spolColumnSize, DoubleMatrix.unit(spolRowSize, spolColumnSize));

    B.setSubMatrix(1, spRowSize, 1, inputDegree, sp.get(1));
    B.setSubMatrix(spRowSize + 1, spRowSize + spolRowSize, 1, inputDegree, spol.get(1));

    C.setSubMatrix(1, outputDegree, 1, spColumnSize, sp.get(2));
    C.setSubMatrix(1, outputDegree, spColumnSize + 1, spColumnSize + spolColumnSize, spol.get(2));

    E.setSubMatrix(1, spRowSize, 1, spColumnSize, DoubleMatrix.unit(spRowSize, spColumnSize));
    E.setSubMatrix(spRowSize + 1, spRowSize + spolRowSize, 1, spColumnSize, leftLowerMatrix);
    E.setSubMatrix(1, spRowSize, spColumnSize + 1, spColumnSize + spolColumnSize, upperRightMatrix);
    E.setSubMatrix(spRowSize + 1, spRowSize + spolRowSize, spColumnSize + 1, spColumnSize + spolColumnSize, spol.get(0));

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D, E}));
  }
}