Tf2des.java

/*
 * Created on 2008/12/16
 * Copyright (C) 2008 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.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoubleMatrixUtil;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.AnyRealRationalPolynomial;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * 伝達関数(係数からなる行列)から状態空間表現に変換するクラスです．
 * 
 * <p>Transfer function to descriptor-state-space conversion
 * 
 * @author Anan
 */
public class Tf2des {

  /**
   * 1入力多出力システム(非プロパー)
   * 
   * <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 transferFunction 伝達関数
   * @return Descriptor DoubleMatrix List{A,B,C,D,E} ディスクリプター表現(descriptor state space representation)
   */
  public static List<DoubleMatrix> tf2des(DoubleRationalPolynomial transferFunction) {
    return tf2des(transferFunction.getNumerator().getCoefficients(), transferFunction.getDenominator().getCoefficients());
  }

  /**
   * 1入力多出力システム(非プロパー)
   * 
   * <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 numeratorMatrix 伝達関数の分子多項式の係数
   * @param denominatorMatrix 伝達関数の分母多項式の係数
   * @return Descriptor DoubleMatrix List{A,B,C,D,E} ディスクリプター表現(descriptor state space representation)
   */
  public static List<DoubleMatrix> tf2des(final DoubleMatrix numeratorMatrix, final DoubleMatrix denominatorMatrix) {
    // Strip leading zeros;
    IntMatrix denIdx = denominatorMatrix.flipLeftRight().compareElementWise(".!=", 0).find(); //$NON-NLS-1$
    DoubleMatrix denominator;
    if (denIdx.length() > 0) {
      denominator = denominatorMatrix.flipLeftRight().getSubVector(denIdx.getIntElement(1), denominatorMatrix.length());
    } else {
      denominator = denominatorMatrix.createOnes(1);
    }

    // Strip leading zeros;
    IntMatrix numIdx = numeratorMatrix.flipLeftRight().compareElementWise(".!=", 0).find(); //$NON-NLS-1$
    DoubleMatrix numerator;
    if (numIdx.length() > 0) {
      numerator = numeratorMatrix.flipLeftRight().getSubVector(numIdx.getIntElement(1), numeratorMatrix.length());
    } else {
      numerator = numeratorMatrix.createZero(1);
      throw new RuntimeException(Messages.getString("Tf2dss.0")); //$NON-NLS-1$
    }

    //    if (numerator.length() < denominator.length()) throw new RuntimeException(Messages.getString("Tf2dss.1")); //$NON-NLS-1$
    //    if (numerator.length() == denominator.length()) throw new RuntimeException(Messages.getString("Tf2dss.2")); //$NON-NLS-1$

    final DoubleMatrix E = createE(numerator.length());
    final DoubleMatrix A = createA(numerator, denominator);
    final DoubleMatrix B = createB(numerator.length());
    final DoubleMatrix C = createC(numerator);
    final DoubleMatrix D = createD();

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

  /**
   * E行列を作成します．
   * 
   * @param degree 次数
   * @return E(Matrix)
   */
  private static DoubleMatrix createE(int degree) {
    final DoubleMatrix subMatrix = new DoubleMatrix(DoubleMatrixUtil.createUnit(degree - 1, degree - 1));
    final DoubleMatrix e = new DoubleMatrix(degree, degree);
    e.setSubMatrix(1, 1, subMatrix, subMatrix);
    //    e.appendDown(new DoubleMatrix(1, degree + 1));
    //    e.appendRight(new DoubleMatrix(degree + 1, 1));
    return e;
  }

  /**
   * A行列を作成します．
   * 
   * @param numeratorMatrix 分子行列
   * @param denominatorMatrix 分母行列
   * @return A(Matrix)
   */
  private static DoubleMatrix createA(DoubleMatrix numeratorMatrix, DoubleMatrix denominatorMatrix) {
    final int nDegree = numeratorMatrix.length() - 1;
    final int dDegree = denominatorMatrix.length() - 1;
    final DoubleMatrix unitMatrix = new DoubleMatrix(DoubleMatrixUtil.createUnit(nDegree, nDegree));
    final DoubleMatrix upperMatrix = new DoubleMatrix(nDegree, 1).appendRight(unitMatrix);
    final DoubleMatrix coefficientsMatrix = denominatorMatrix.flipLeftRight().unaryMinus();
    final DoubleMatrix lowerMatrix = coefficientsMatrix.appendRight(new DoubleMatrix(1, nDegree - dDegree));
    final DoubleMatrix a = upperMatrix.appendDown(lowerMatrix);
    return a;
  }

  /**
   * B行列を作成します．
   * 
   * @param degree 次数
   * @return B(Matrix)
   */
  private static DoubleMatrix createB(int degree) {
    final DoubleMatrix subMatrix = new DoubleMatrix(degree - 1, 1);
    final DoubleMatrix b = subMatrix.appendDown(new DoubleMatrix(new double[] {1}));
    return b;
  }

  /**
   * C行列を作成します．
   * 
   * @param numeratorMatrix 分子行列
   * @return C(Matrix)
   */
  private static DoubleMatrix createC(DoubleMatrix numeratorMatrix) {
    return numeratorMatrix.flipLeftRight();
  }

  /**
   * D行列を作成します．
   * 
   * @return D(Matrix)
   */
  private static DoubleMatrix createD() {
    return new DoubleMatrix(1, 1);
  }

  /**
   * 1入力多出力システム(非プロパー)
   * 
   * <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 numeratorMatrix 伝達関数の分子多項式の係数
   * @param denominatorMatrix 伝達関数の分母多項式の係数
   * @return Descriptor DoubleMatrix List{A,B,C,D,E} ディスクリプター表現(descriptor state space representation)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   */
  public static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> List<RM> tf2des(
      final RM numeratorMatrix, final RM denominatorMatrix) {
    // Strip leading zeros;
    IntMatrix denIdx = denominatorMatrix.flipLeftRight().compareElementWise(".!=", 0).find(); //$NON-NLS-1$
    RM denominator;
    if (denIdx.length() > 0) {
      denominator = denominatorMatrix.flipLeftRight().getSubVector(denIdx.getIntElement(1), denominatorMatrix.length());
    } else {
      denominator = denominatorMatrix.createOnes(1);
    }

    // Strip leading zeros;
    IntMatrix numIdx = numeratorMatrix.flipLeftRight().compareElementWise(".!=", 0).find(); //$NON-NLS-1$
    RM numerator;
    if (numIdx.length() > 0) {
      numerator = numeratorMatrix.flipLeftRight().getSubVector(numIdx.getIntElement(1), numeratorMatrix.length());
    } else {
      numerator = numeratorMatrix.createZero(1);
      throw new RuntimeException(Messages.getString("Tf2dss.0")); //$NON-NLS-1$
    }

    //    if (numerator.length() < denominator.length()) throw new RuntimeException(Messages.getString("Tf2dss.1")); //$NON-NLS-1$
    //    if (numerator.length() == denominator.length()) throw new RuntimeException(Messages.getString("Tf2dss.2")); //$NON-NLS-1$

    RS sunit = numerator.getElement(1).createUnit();

    final RM E = createE(numerator.length(), sunit);
    final RM A = createA(numerator, denominator, sunit);
    final RM B = createB(numerator.length(), sunit);
    final RM C = createC(numerator, sunit);
    final RM D = createD(sunit);

    List<RM> abcde = new ArrayList<>();
    abcde.add(A);
    abcde.add(B);
    abcde.add(C);
    abcde.add(D);
    abcde.add(E);

    return abcde;
  }

  /**
   * A行列を作成します．
   * 
   * @param numeratorMatrix 分子行列
   * @param denominatorMatrix 分母行列
   * @return A(Matrix)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   * @param sunit unit of scalar
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM createA(
      RM numeratorMatrix, RM denominatorMatrix, RS sunit) {
    final int nDegree = numeratorMatrix.length() - 1;
    final int dDegree = denominatorMatrix.length() - 1;
    final RM unitMatrix = sunit.createUnitGrid(nDegree, nDegree);
    final RM upperMatrix = sunit.createZeroGrid(nDegree, 1).appendRight(unitMatrix);
    final RM coefficientsMatrix = denominatorMatrix.flipLeftRight().unaryMinus();
    final RM lowerMatrix = coefficientsMatrix.appendRight(sunit.createZeroGrid(1, nDegree - dDegree));
    final RM a = upperMatrix.appendDown(lowerMatrix);
    return a;
  }

  /**
   * B行列を作成します．
   * 
   * @param degree 次数
   * @return B(Matrix)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   * @param sunit unit of scalar
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM createB(
      int degree, RS sunit) {
    final RM subMatrix = sunit.createZeroGrid(degree - 1, 1);
    final RM b = subMatrix.appendDown(sunit.createUnitGrid(1));
    return b;
  }

  /**
   * C行列を作成します．
   * 
   * @param numeratorMatrix 分子行列
   * @return C(Matrix)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   * @param sunit unit of scalar
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM createC(
      RM numeratorMatrix, RS sunit) {
    return numeratorMatrix.flipLeftRight();
  }

  /**
   * D行列を作成します．
   * 
   * @return D(Matrix)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   * @param sunit unit of scalar
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM createD(
      RS sunit) {
    return sunit.createZeroGrid(1, 1);
  }

  /**
   * E行列を作成します．
   * 
   * @param degree 次数
   * @return E(Matrix)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   * @param sunit unit of scalar
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM createE(
      int degree, RS sunit) {
    final RM subMatrix = sunit.createUnitGrid(degree - 1, degree - 1);
    final RM e = sunit.createZeroGrid(degree, degree);
    e.setSubMatrix(1, 1, subMatrix, subMatrix);
    return e;
  }

  /**
   * 1入力多出力システム(非プロパー)
   * 
   * <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 transferFunction 伝達関数
   * @return Descriptor DoubleMatrix List{A,B,C,D,E} ディスクリプター表現(descriptor state space representation)
   * @param <RS> type of real scalar
   * @param <RM> type of real matrix
   * @param <CS> type of complex scalar
   * @param <CM> type of complex matrix
   */
  public static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> List<RM> tf2des(
      AnyRealRationalPolynomial<RS, RM, CS, CM> transferFunction) {
    return tf2des(transferFunction.getNumerator().getCoefficients(), transferFunction.getDenominator().getCoefficients());
  }

}