Des2tfm.java

/*
 * Created on 2009/01/28
 * Copyright (C) 2009 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.AnyRealPolynomialMatrix;
import org.mklab.nfc.matrix.AnyRealRationalPolynomialMatrix;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.DoublePolynomialMatrix;
import org.mklab.nfc.matrix.DoubleRationalPolynomialMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.AnyRealPolynomial;
import org.mklab.nfc.scalar.AnyRealRationalPolynomial;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * ディスクリプタ表現から伝達関数行列(有理多項式行列)に変換するクラスです。 <p>copy of Ss2tf.java</p>
 * 
 * @author Anan
 * 
 */
public class Des2tfm {

  /**
   * @param A ゲイン行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param E ディスクリプター行列
   * @return TransferFunctionMatrix
   */
  public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E) {
    DoubleMatrix numericalA = A;
    DoubleNumber eps = numericalA.getElement(1, 1).getMachineEpsilon();
    double defaultTolerance = numericalA.frobNorm().multiply(eps).doubleValue();
    boolean defaultSimplify = true;
    return des2tfm(A, B, C, D, E, defaultTolerance, defaultSimplify);
  }

  /**
   * @param A ゲイン行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param E ディスクリプター行列
   * @param simplify 簡単化するならばtrue
   * @return TransferFunctionMatrix
   */
  public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E, boolean simplify) {
    DoubleMatrix numericalA = A;
    DoubleNumber eps = numericalA.getElement(1, 1).getMachineEpsilon();
    double defaultTolerance = numericalA.frobNorm().multiply(eps).doubleValue() * A.getRowSize();
    return des2tfm(A, B, C, D, E, defaultTolerance, simplify);
  }

  /**
   * @param A ゲイン行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param E ディスクリプター行列
   * @param tolerance 許容誤差
   * @return TransferFunctionMatrix
   */
  public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E, double tolerance) {
    boolean defaultSimplify = true;
    return des2tfm(A, B, C, D, E, tolerance, defaultSimplify);
  }

  /**
   * @param A ゲイン行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param E ディスクリプター行列
   * @param tolerance 許容誤差
   * @param simplify 簡単化するならばtrue
   * @return TransferFunctionMatrix
   */
  public static DoubleRationalPolynomialMatrix des2tfm(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, DoubleMatrix E, double tolerance, boolean simplify) {
    DoubleRationalPolynomialMatrix simpG = new DoubleRationalPolynomialMatrix();
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    List<DoubleMatrix> para = new WeierstrassForm<DoubleNumber, DoubleMatrix, DoubleComplexNumber, DoubleComplexMatrix>().weiestrassformByHessenberg(Arrays.asList(new DoubleMatrix[] {A, B, C, D, E}),
        new DoubleNumber(tolerance));
    DoubleMatrix a = para.get(0);
    DoubleMatrix b = para.get(1);
    DoubleMatrix c = para.get(2);
    //DoubleMatrix d = para.get(3);
    DoubleMatrix e = para.get(4);

    int zeroCount = 0;

    for (int i = 1; i <= E.getColumnSize(); i++) {
      if (e.getElement(i, i).isZero(tolerance)) {
        zeroCount = i - 1;
        break;
      }
    }

    DoubleMatrix As = a.getSubMatrix(1, zeroCount, 1, zeroCount);
    DoubleMatrix Af = e.getSubMatrix(1 + zeroCount, e.getColumnSize(), 1 + zeroCount, e.getColumnSize());

    DoubleMatrix Bs = b.getSubMatrix(1, zeroCount, 1, b.getColumnSize());
    DoubleMatrix Bf = b.getSubMatrix(1 + zeroCount, b.getRowSize(), 1, b.getColumnSize());

    DoubleMatrix Cs = c.getSubMatrix(1, c.getRowSize(), 1, zeroCount);
    DoubleMatrix Cf = c.getSubMatrix(1, c.getRowSize(), zeroCount + 1, c.getColumnSize());

    DoubleRationalPolynomialMatrix Gs = Ss2tfm.ss2tfm(As, Bs, Cs, D);

    DoubleMatrix[] invIsAfCofficient = new DoubleMatrix[Af.getColumnSize()];
    invIsAfCofficient[0] = Af.createUnit();
    for (int i = 1; i < invIsAfCofficient.length; i++) {
      invIsAfCofficient[i] = Af;
      for (int j = 1; j < i; j++) {
        invIsAfCofficient[i] = invIsAfCofficient[i].multiply(Af);
      }
    }

    DoublePolynomialMatrix invIsAf = new DoublePolynomialMatrix(Af.getRowSize(), Af.getColumnSize());

    for (int column = 1; column <= invIsAf.getColumnSize(); column++) {
      for (int row = 1; row <= invIsAf.getRowSize(); row++) {
        DoublePolynomial pol = new DoublePolynomial(new double[invIsAf.length()]);
        for (int order = 0; order < invIsAf.length(); order++) {
          pol.setCoefficient(order, invIsAfCofficient[order].getElement(row, column));
        }
        pol.simplify(tolerance);
        invIsAf.setElement(row, column, pol);
      }
    }

    DoubleRationalPolynomialMatrix Gf = new DoubleRationalPolynomialMatrix(invIsAf);

    Gf = new DoubleRationalPolynomialMatrix(Cf).multiply(Gf).multiply(new DoubleRationalPolynomialMatrix(Bf));

    DoubleRationalPolynomialMatrix G = Gs.subtract(Gf);

    final DoubleRationalPolynomialMatrix simplifiedG = G.createClone();
    for (int row = 1; row <= G.getRowSize(); row++) {
      for (int column = 1; column <= G.getColumnSize(); column++) {
        DoubleRationalPolynomial g = G.getElement(row, column);
        DoublePolynomial numerator = g.getNumerator();
        DoublePolynomial denominator = g.getDenominator();
        if (simplify) {
          numerator.simplify(tolerance);
          denominator.simplify(tolerance);
        }
        simplifiedG.setElement(row, column, new DoubleRationalPolynomial(numerator, denominator));
      }
    }
    simpG = simplifiedG;

    return simpG;
  }

  /**
   * @param A ゲイン行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param E ディスクリプター行列
   * @param tolerance 許容誤差
   * @param simplify 簡単化するならばtrue
   * @return TransferFunctionMatrix
   * @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>> AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> des2tfm(
      RM A, RM B, RM C, RM D, RM E, RS tolerance, boolean simplify) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    List<RM> abcde = new ArrayList<>();
    abcde.add(A);
    abcde.add(B);
    abcde.add(C);
    abcde.add(D);
    abcde.add(E);
    List<RM> para = new WeierstrassForm<RS, RM, CS, CM>().weiestrassformByHessenberg(abcde, tolerance);

    int zeroCount = 0;

    for (int i = 1; i <= E.getColumnSize(); i++) {
      if (para.get(4).getElement(i, i).isZero(tolerance)) {
        zeroCount = i - 1;
        break;
      }
    }

    RM As = para.get(0).getSubMatrix(1, zeroCount, 1, zeroCount);
    RM Af = para.get(4).getSubMatrix(1 + zeroCount, para.get(4).getColumnSize(), 1 + zeroCount, para.get(4).getColumnSize());

    RM Bs = para.get(1).getSubMatrix(1, zeroCount, 1, para.get(1).getColumnSize());
    RM Bf = para.get(1).getSubMatrix(1 + zeroCount, para.get(1).getRowSize(), 1, para.get(1).getColumnSize());

    RM Cs = para.get(2).getSubMatrix(1, para.get(2).getRowSize(), 1, zeroCount);
    RM Cf = para.get(2).getSubMatrix(1, para.get(2).getRowSize(), zeroCount + 1, para.get(2).getColumnSize());

    AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> Gs = Ss2tfm.ss2tfm(As, Bs, Cs, D);

    List<RM> invIsAfCofficient = new ArrayList<>(Af.getColumnSize());
    invIsAfCofficient.add(Af.createUnit());
    for (int i = 1; i < invIsAfCofficient.size(); i++) {
      invIsAfCofficient.add(Af);
      for (int j = 1; j < i; j++) {
        invIsAfCofficient.set(i, invIsAfCofficient.get(i).multiply(Af));
      }
    }

    AnyRealPolynomial<RS, RM, CS, CM>[][] af = Gs.getElement(1, 1).getNumerator().createArray(Af.getRowSize(), Af.getColumnSize());
    AnyRealPolynomialMatrix<RS, RM, CS, CM> invIsAf = new AnyRealPolynomialMatrix<>(Af.getRowSize(), Af.getColumnSize(), af);

    for (int column = 1; column <= invIsAf.getColumnSize(); column++) {
      for (int row = 1; row <= invIsAf.getRowSize(); row++) {
        RS[] cc = tolerance.createArray(invIsAf.length());
        AnyRealPolynomial<RS, RM, CS, CM> pol = new AnyRealPolynomial<>(cc);
        for (int order = 0; order < invIsAf.length(); order++) {
          pol.setCoefficient(order, invIsAfCofficient.get(order).getElement(row, column));
        }
        pol.simplify();
        invIsAf.setElement(row, column, pol);
      }
    }

    AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> Gf = invIsAf.toRational();

    Gf = new AnyRealPolynomialMatrix<>(Cf).toRational().multiply(Gf).multiply(new AnyRealPolynomialMatrix<>(Bf).toRational());

    AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> G = Gs.subtract(Gf);

    final AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> simplifiedG = G.createClone();
    for (int row = 1; row <= G.getRowSize(); row++) {
      for (int column = 1; column <= G.getColumnSize(); column++) {
        AnyRealRationalPolynomial<RS, RM, CS, CM> g = G.getElement(row, column);
        AnyRealPolynomial<RS, RM, CS, CM> numerator = g.getNumerator();
        AnyRealPolynomial<RS, RM, CS, CM> denominator = g.getDenominator();
        if (simplify) {
          numerator.simplify(tolerance);
          denominator.simplify(tolerance);
        }
        simplifiedG.setElement(row, column, new AnyRealRationalPolynomial<>(numerator, denominator));
      }
    }

    AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> simpG = simplifiedG;

    return simpG;
  }

  /**
   * @param A ゲイン行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param E ディスクリプター行列
   * @param simplify 簡単化するならばtrue
   * @return TransferFunctionMatrix
   * @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>> AnyRealRationalPolynomialMatrix<RS, RM, CS, CM> des2tfm(
      RM A, RM B, RM C, RM D, RM E, boolean simplify) {
    RM numericalA = A;
    RS eps = numericalA.getElement(1, 1).getMachineEpsilon();
    RS defaultTolerance = numericalA.frobNorm().multiply(eps).multiply(A.getRowSize());
    return des2tfm(A, B, C, D, E, defaultTolerance, simplify);
  }

}