Ss2tf.java

/*
 * $Id: Ss2tf.java,v 1.25 2008/03/24 14:57:16 koga Exp $
 *
 * Copyright (C) 2004 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.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.matrix.Makepoly;


/**
 * 状態空間表現から伝達関数(係数からなる行列)に変換するクラスです。
 * 
 * <p>State-space to transfer function conversion
 * 
 * @author koga
 * @version $Revision: 1.25 $
 * @see org.mklab.tool.control.Ss2tfm
 * @see org.mklab.tool.control.Ss2tfn
 * @see org.mklab.tool.control.Ss2zp
 * @see org.mklab.tool.control.Tf2ss
 */
public class Ss2tf {

  /**
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @return 伝達関数 (transfer function)
   */
  public static List<DoubleMatrix> ss2tf(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D) {
    int inputNumber = 1;
    return ss2tf(A, B, C, D, inputNumber);
  }

  /**
   * 状態空間表現が
   * 
   * <pre><code> . x = Ax + Bu y = Cx + Du </code></pre>
   * 
   * であるシステムの<code>i</code>番目の入力から出力までの伝達関数
   * 
   * <pre><code>
   * 
   * NUM(s) -1 G(s) = -------- = C(sI-A) B + D den(s) </code></pre>
   * 
   * の分子行列多項式の係数行列<code>NUM</code>と分母多項式の係数 <code>den</code>を求めます。
   * 
   * <p><code>NUM</code>の大きさは、<code>Rows(C)×Rows(A)</code>、 <code>den</code>の大きさは、<code>1-by-Rows(A)</code>です。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param inputNumber 入力番号
   * @return 伝達関数 (transfer function)
   */
  public static List<DoubleMatrix> ss2tf(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D, int inputNumber) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    int outputSize = C.getRowSize();

    DoubleMatrix numerators;
    if (A.isReal() && B.isReal() && C.isReal() && D.isReal()) {
      numerators = A.createZero(outputSize, A.getRowSize() + 1);
    } else {
      DoubleMatrix zero = A.createZero(outputSize, A.getRowSize() + 1);
      numerators = zero;
    }

    DoubleMatrix Bi = B.getColumnVector(inputNumber);
    DoubleMatrix Di = D.getColumnVector(inputNumber);
    DoubleMatrix denominator = Makepoly.makepoly(A).getCoefficients();

    for (int j = 1; j <= outputSize; j++) {
      if (C.length() == 0) {
        numerators.setElement(j, 1, Di.getElement(j));
      } else {
        DoubleMatrix tmp1 = A.subtract(Bi.multiply(C.getRowVector(j)));
        DoubleMatrix tmp2 = Makepoly.makepoly(tmp1).getCoefficients();
        DoubleMatrix tmp3 = tmp2.add(denominator.multiply(Di.getElement(j).subtract(1)));
        numerators.setRowVector(j, tmp3);
      }
    }

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {numerators, denominator}));
    //return new MatxList(new Object[] {numerators, denominator});
  }

  /**
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @return 伝達関数 (transfer function)
   * @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> ss2tf(
      RM A, RM B, RM C, RM D) {
    int inputNumber = 1;
    return ss2tf(A, B, C, D, inputNumber);
  }

  /**
   * 状態空間表現が
   * 
   * <pre><code> . x = Ax + Bu y = Cx + Du </code></pre>
   * 
   * であるシステムの<code>i</code>番目の入力から出力までの伝達関数
   * 
   * <pre><code>
   * 
   * NUM(s) -1 G(s) = -------- = C(sI-A) B + D den(s) </code></pre>
   * 
   * の分子行列多項式の係数行列<code>NUM</code>と分母多項式の係数 <code>den</code>を求めます。
   * 
   * <p><code>NUM</code>の大きさは、<code>Rows(C)×Rows(A)</code>、 <code>den</code>の大きさは、<code>1-by-Rows(A)</code>です。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param inputNumber 入力番号
   * @return 伝達関数 (transfer function)
   * @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> ss2tf(
      RM A, RM B, RM C, RM D, int inputNumber) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    int outputSize = C.getRowSize();

    RM numerators;
    if (A.isReal() && B.isReal() && C.isReal() && D.isReal()) {
      numerators = A.createZero(outputSize, A.getRowSize() + 1);
    } else {
      RM zero = A.createZero(outputSize, A.getRowSize() + 1);
      numerators = zero;
    }

    RM Bi = B.getColumnVector(inputNumber);
    RM Di = D.getColumnVector(inputNumber);
    RM denominator = Makepoly.makepoly(A).getCoefficients();

    for (int j = 1; j <= outputSize; j++) {
      if (C.length() == 0) {
        numerators.setElement(j, 1, Di.getElement(j));
      } else {
        RM tmp1 = A.subtract(Bi.multiply(C.getRowVector(j)));
        RM tmp2 = Makepoly.makepoly(tmp1).getCoefficients();
        RM tmp3 = tmp2.add(denominator.multiply(Di.getElement(j).subtract(1)));
        numerators.setRowVector(j, tmp3);
      }
    }

    List<RM> nd = new ArrayList<>();
    nd.add(numerators);
    nd.add(denominator);
    return nd;
  }

}