Ss2zp.java

/*
 * $Id: Ss2zp.java,v 1.31 2008/03/24 23:45:43 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.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.tool.matrix.Diag;
import org.mklab.tool.matrix.Matrix4;


/**
 * 状態空間表現からシステムの極とゼロ点の集合に変換するクラスです。
 * 
 * <p>State-space to zero-pole conversion
 * 
 * @author koga
 * @version $Revision: 1.31 $
 * @see org.mklab.tool.control.Ss2tf
 * @see org.mklab.tool.control.Ss2tfn
 * @see org.mklab.tool.control.Ss2tfm
 * @see org.mklab.tool.control.Zp2ss
 */
public class Ss2zp {

  /**
   * 状態空間表現が
   * 
   * <pre><code> . x = Ax + Bu y = Cx + Du </code></pre>
   * 
   * であり、その伝達関数が
   * 
   * <pre><code> -1 (s-z1)(s-z2)...(s-zn) G(s) = C(sI-A) B + D = k --------------------- (s-p1)(s-p2)...(s-pn) </code></pre>
   * 
   * であるシステムのゼロ点<code>z</code>, 極<code>p</code>, ゲイン<code>k</code>を返します。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @return {z, po, k} (ゼロ点, 極, ゲイン) (zero-pole representation)
   */
  public static List<DoubleComplexMatrix> ss2zp(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D) {
    int inputNumber = 1;
    return ss2zp(A, B, C, D, inputNumber);
  }

  /**
   * <code>i</code>番目の入力から出力までの伝達関数のゼロ点、極、ゲインを求めます。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @param inputNumber 入力番号
   * @return {z, po, k} (ゼロ点, 極, ゲイン) (zero-pole representation)
   */
  public static List<DoubleComplexMatrix> ss2zp(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 stateSize = A.getRowSize();
    int outputSize = C.getRowSize();

    DoubleMatrix Bi = B.getSubMatrix(1, B.getRowSize(), inputNumber, inputNumber);
    DoubleMatrix Di = D.getSubMatrix(1, D.getRowSize(), inputNumber, inputNumber);

    // zeros
    final DoubleNumber unit = A.getElement(1, 1);
    final DoubleMatrix one = A.createOnes(stateSize, outputSize);
    DoubleComplexMatrix z = new DoubleComplexMatrix(one.multiply(unit.getInfinity()));

    for (int j = 1; j <= outputSize; j++) {
      DoubleMatrix tmp1 = Matrix4.matrix4(A, Bi, C.getSubMatrix(j, j, 1, C.getColumnSize()), Di.getSubMatrix(j, j, 1, 1));
      DoubleMatrix tmp2 = Diag.diag(A.createUnit(stateSize), A.createZero(1, 1));
      DoubleComplexMatrix zz = tmp1.eigenValue(tmp2);
      zz = zz.getSubVector(zz.isNanElementWise().notElementWise().andElementWise(zz.isFiniteElementWise()).find());
      int nz;
      if ((nz = zz.length()) > 0) {
        z.setSubMatrix(1, nz, inputNumber, inputNumber, zz);
      }
    }

    // gains
    DoubleMatrix k = Di;
    DoubleMatrix CA = C;
    int j = 0;
    while (k.compareElementWise(".==", 0).anyTrue()) { //$NON-NLS-1$
      IntMatrix idx = k.compareElementWise(".==", 0).find(); //$NON-NLS-1$
      if (j > stateSize) {
        k.setSubVector(idx, A.createZero(idx.length(), 1));
        break;
      }
      k.setSubVector(idx, CA.multiply(Bi).getSubVector(idx));
      CA = CA.multiply(A);
      j++;
    }

    // poles
    DoubleComplexMatrix po = A.eigenValue();

    return new ArrayList<>(Arrays.asList(new DoubleComplexMatrix[] {z, po, new DoubleComplexMatrix(k)}));
    //return new MatxList(new Object[] {z, po, k});
  }

}