Tzero.java

/*
 * $Id: Tzero.java,v 1.43 2008/03/15 00:23:43 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.NormType;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.tool.matrix.Diag;
import org.mklab.tool.matrix.Matrix4;


/**
 * 伝達ゼロ点を求めるクラスです。
 * 
 * <p>Transmission zeros
 * 
 * @author koga
 * @version $Revision: 1.43 $
 */
public class Tzero {

  /**
   * システム
   * 
   * <pre><code>
   * 
   * . x = Ax + Bu y = Cx + Du
   * 
   * </code></pre>
   * 
   * の伝達ゼロ点を求めます。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param D ゲイン行列
   * @return 伝達ゼロ点 (transmission zeros)
   */
  public static DoubleComplexMatrix tzero(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix D) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B, C, D)).length() > 0) {
      throw new RuntimeException(message);
    }

    int stateSize = A.getRowSize();
    int inputSize = B.getColumnSize();
    int outputSize = C.getRowSize();

    DoubleMatrix AA = Matrix4.matrix4(A, B, C, D);

    DoubleComplexMatrix tz;
    if (inputSize == outputSize) {
      DoubleMatrix BB = Diag.diag(A.createUnit(stateSize), A.createZero(outputSize, inputSize));
      tz = AA.eigenValue(BB);
      tz = tz.getSubVector(tz.isNanElementWise().notElementWise().andElementWise(tz.isFiniteElementWise()).find());
    } else {
      DoubleNumber mu = AA.norm(NormType.ONE);

      DoubleMatrix A1, A2;
      if (inputSize > outputSize) {
        A1 = AA.appendDown(A.createUniformRandom(inputSize - outputSize, stateSize + inputSize).subtractElementWise(0.5).multiply(mu));
        A2 = AA.appendDown(A.createUniformRandom(inputSize - outputSize, stateSize + inputSize).subtractElementWise(0.5).multiply(mu));
      } else {
        A1 = AA.appendRight(A.createUniformRandom(stateSize + outputSize, outputSize - inputSize).subtractElementWise(0.5).multiply(mu));
        A2 = AA.appendRight(A.createUniformRandom(stateSize + outputSize, outputSize - inputSize).subtractElementWise(0.5).multiply(mu));
      }
      DoubleMatrix BB = Diag.diag(A.createUnit(stateSize), A.createZero(A1.getRowSize() - stateSize, inputSize));
      DoubleComplexMatrix g1 = A1.eigenValue(BB);
      DoubleComplexMatrix g2 = A2.eigenValue(BB);
      g1 = g1.getSubVector(g1.isNanElementWise().notElementWise().andElementWise(g1.isFiniteElementWise()).find());
      g2 = g2.getSubVector(g2.isNanElementWise().notElementWise().andElementWise(g2.isFiniteElementWise()).find());

      DoubleComplexNumber unit = g1.getElement(1);

      tz = g1.createZero(0, 0);
      for (int i = 1; i <= g1.length(); i++) {
        if (g2.unaryMinus().addElementWise(g1.getElement(i)).absElementWise().getRealPart().compareElementWise(".<", mu.multiply(mu.getMachineEpsilon().sqrt())).anyTrue()) { //$NON-NLS-1$
          tz = tz.appendDown(unit.createGrid(new DoubleComplexNumber[] {g1.getElement(i)}));
        }
      }
    }
    return tz;
  }

}